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Abstract 

Robot  mapping  remains  one  of  the  most  challenging  problems  in  robot  pro¬ 
gramming.  Most  successful  methods  use  some  form  of  occupancy  grid  for  represent¬ 
ing  a  mapped  region.  An  occupancy  grid  is  a  two  dimensional  array  in  which  the 
array  cells  represents  (x,  y )  coordinates  of  a  cartesian  map.  This  approach  becomes 
problematic  in  mapping  large  environments  as  the  map  quickly  becomes  too  large 
for  processing  and  storage. 

Rather  than  storing  the  map  as  an  occupancy  grid,  our  robot  (equipped  with 
ultrasonic  sonars)  views  the  world  as  a  series  of  connected  spaces.  These  spaces 
are  initially  mapped  as  an  occupancy  grid  in  a  room-by-room  fashion  using  a  mod¬ 
ified  version  of  the  Histogram  In  Motion  Mapping  (HIMM)  algorithm  extended  in 
this  thesis.  As  the  robot  leaves  a  space,  denoted  by  passing  through  a  doorway,  it 
converts  the  grid  to  a  polygonal  representation  using  a  novel  edge  detection  tech¬ 
nique.  Then,  it  stores  the  polygonal  representation  as  rooms  and  hallways  in  a  set  of 
Absolute  Space  Representations  (ASRs)  representing  the  space  connections.  Using 
this  representation  makes  navigation  and  localization  easier  for  the  robot  to  process. 
The  system  also  performs  localization  on  the  simplified  cognitive  version  of  the  map 
using  an  iterative  method  of  estimating  the  maximum  likelihood  of  the  robot’s  cor¬ 
rect  position.  This  is  accomplished  using  the  Expectation  Maximization  algorithm. 
Treating  vector  directions  from  the  polygonal  map  as  a  Gaussian  distribution,  the 
Expectation  Maximization  algorithm  is  applied,  for  the  first  time,  to  find  the  most 
probable  correct  pose  while  using  a  cognitive  mapping  approach. 


Concurrent  Cognitive  Mapping  and  Localization 
using  Expectation  Maximization 


I.  Introduction 

Learning  a  map  of  its  environment  presents  a  complex  problem  for  robots.  The 
problem  begins  with  getting  the  robot  to  move  in  such  a  fashion  that  it  explores 
the  entire  environment,  insuring  a  complete  map  is  constructed.  While  the  robot 
explores  and  records  real  time  sensor  data,  it  uses  that  data  (range  and  motor  data) 
to  form  a  map  of  the  explored  region.  These  maps  allow  the  robot  to  develop  a 
sense  of  a  place  and  localize  themselves  in  the  world.  Memory  space  limitations 
(memory  is  finite  in  any  robot)  require  creative  solutions  for  the  representation  of 
the  map  itself  when  mapping  large  environments.  These  maps  can  take  on  a  number 
of  internal  representations  from  a  grid  to  a  general  graph  of  interconnected  spaces; 
our  method  combines  the  best  of  both  approaches,  allowing  for  greater  efficiency 
performing  planning  and  localization. 

An  associated  problem  to  learning  the  map  includes  localization  of  the  robot. 
Accurate  localization  is  a  prerequisite  for  building  a  good  map  and  having  an  accurate 
map  is  essential  for  good  localization.  Simultaneous  Localization  And  Mapping 
(SLAM)  is  a  critical  underlying  factor  for  any  successful  mobile  robot  navigation. 

1.1  Overview 

The  process  of  learning  a  map  breaks  into  three  main  categories;  mapping, 
localization  and  exploration.  Each  of  these  categories  function  independently  but 
inevitably  depend  on  each  other.  A  robot  maps  a  region  by  creating  a  representation 
of  the  area  it  has  explored.  The  robot  can  perform  exploration  without  knowledge 
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of  where  it  has  been,  but  if  so,  is  no  more  than  a  random  wandering  routine  just 
as  prone  to  revisiting  an  already  explored  region  than  not.  Localization  without 
consideration  of  observation  data,  called  dead  reckoning,  is  not  very  accurate  as  the 
tires  tend  to  slip  and  rotation  sensors  are  not  perfectly  accurate.  For  these  reasons,  it 
is  important  to  use  observation  data  along  with  dead  reckoning  data  to  gain  a  more 
accurate  estimation  of  the  robot’s  location.  Thus,  there  is  a  mutual  dependence 
between  mapping,  localizing  and  exploring.  Each  of  these  categories  has  its  own 
associated  challenges  as  explained  in  the  following  sections,  starting  with  mapping. 

1.1.1  Mapping.  The  representation  of  the  map  is  a  key  factor  of  robot  map¬ 
ping.  Implementations  of  robot  mapping  represent  map  data  as  either  an  occupancy 
grid  [26]  [4]  or  a  topological  network  [17]  [16]  [13].  An  occupancy  grid  is  a  matrix  of 
cells  where  cells  containing  a  value  greater  than  0.0  indicates  a  belief  that  an  obstacle 
resides  in  space  represented  by  the  the  cell.  Using  a  matrix  in  this  fashion  requires 
the  reservation  of  a  large  amount  of  memory  for  representing  the  entire  maximum 
map  size.  A  topological  map  is  one  in  which  objects  are  stored  with  respect  to  each 
other  based  on  a  robot  centric  point  of  view.  This  representation  makes  it  easy  to 
use  memory  only  as  needed,  generating  more  efficient  maps,  but  introduces  imple¬ 
mentation  difficulties  and  cannot  easily  convert  to  human-understandable  maps. 

Generally,  the  robot  is  equipped  with  multiple  sensors,  and  as  sensor  data 
accumulates,  the  representation  of  the  map  is  developed  from  the  fusion  of  that 
data.  This  process  is  usually  referred  to  as  sensor  fusion  [26]. 

Occupancy  grids  can  be  either  discrete  [4]  or  probabilistic  [26]  [27].  Discrete 
grids  produce  maps  by  incrementing  and  decrementing  cell  values.  Probabilistic 
methods  calculate  the  probability  of  a  cell  being  occupied  or  empty  with  each  update 
using  Bayes  probability  theorem  [26] .  Another  technique  incorporates  the  Dempster 
Shafer-Bclicf  theorem  [27]  to  find  the  belief  of  the  occupancy  status  of  a  cell  by 
representing  each  cell’s  occupation  with  Shafer’s  theory  of  belief  and  combining  the 
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multiple  sensors  with  Dempster’s  combination  theory.  Probabilistic  approaches  tend 
to  generate  more  accurate  and  detailed  maps  than  of  the  discrete  methods,  but  are 
slower  due  to  the  increase  in  the  number  of  calculations. 

Topological  mapping,  a  more  recent  mapping  approach,  maps  the  world  much 
like  a  graph  with  nodes  and  edges  as  paths  between  locations.  We  can  separate  the 
map  into  rooms  or  spaces  and  assign  them  as  nodes  in  a  graph  connected  by,  in 
the  case  of  rooms;  the  exits,  and  in  the  case  of  spaces;  the  location  that  separates 
the  spaces.  The  issue  still  remains  of  how  to  represent  the  rooms  or  spaces.  In  this 
thesis,  the  robot  creates  the  room  and  spaces  representation  as  a  set  of  line  segments 
or  polylines  maintaining  geometric  spacial  elements.  This  data  representation  sig¬ 
nificantly  reduces  the  size  of  the  data  structure  and  also  lends  itself  to  faster  path 
planning,  a  more  human  centric  representation  and  the  ability  to  localize  without  us¬ 
ing  local  landmarks.  However,  generating  the  polygonal  representation  with  sonars 
remains  difficult  since  the  noise  generated  by  the  sonars  significantly  complicates  the 
process. 

We  overcome  the  issue  of  sonar  noise  by  using  one  of  the  sensor  fusion  tech¬ 
niques  (discussed  above)  with  a  grid  representation  first,  then  converting  the  grid 
representation  to  a  topological  map.  This  process  starts  by  creating  a  histogram  map 
or  occupancy  grid  from  the  sonar  input/data.  Using  a  new  technique  of  filtering  the 
data  in  the  histogram  (developed  in  this  thesis),  which  reduces  the  histogram  wall 
thickness  from  thickness  of  as  much  as  10  cells  to  one,  the  data  is  cleaned  and  then 
converted  to  a  list  of  vertices  and  filtered  using  the  Douglas-Peucker  line  reduction 
algorithm  [9].  The  remaining  set  of  lines  represents  the  original  map  of  the  room. 
These  lines,  along  with  other  topological  information,  are  stored  in  an  Absolute 
Space  Representation  (ASR)  data  structure  and  used  later  to  form  a  complete  map. 
This  process  repeats  for  each  room  or  space  visited. 
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1.1.2  Localization.  As  the  robot  maps  its  environment,  errors  accumulate 
within  the  robots  dead  reckoning  system  causing  incorrect  pose  data  and  conse¬ 
quently  the  map  to  skew  (Fig.  1.1).  The  coordinates  at  which  the  robot  believes 
itself  to  be  located  is  the  robots  pose.  Localization  is  the  process  of  determining 
where  in  the  environment  the  robot  is  located,  and  the  process  of  correcting  errors 
in  the  robot’s  pose. 


Figure  1.1  This  map  shows  skewing  caused  by  dead  reckoning  error.  The  room  in 
the  middle  is  overlapping  with  the  large  room  to  right. 

Localization  consists  of  two  broad  categories:  local  and  global.  Local  tech¬ 
niques  compensate  for  odometer  errors  and  require  that  the  initial  location  of  the 
robot  is  approximately  known.  Localization  in  this  fashion  progresses  iteratively,  i.e. 
after  some  fixed  amount  of  distance  traveled,  the  robot  analyzes  motor  and  sensor 
data  to  determine  an  adjustment  of  the  robot’s  pose.  Local  techniques  are  weak  in 
that  the  robot  usually  cannot  recover  true  pose  information  if  the  local  techniques 
lose  track  of  the  robot’s  immediate  position.  Global  techniques  are  stronger  as  they 
localize  the  robot  without  any  prior  knowledge  about  its  initial  position.  They  can 
also  handle  the  kidnapped  robot  problem ,  wherein  a  robot  is  kidnapped  and  carried 
to  some  unknown  location.  This  technique,  if  no  prior  map  is  provided,  waits  until 
the  robot  completes  mapping  then  determines  the  most  likely  map  based  on  all  the 
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data.  If  a  map  is  provided,  the  robot  is  able  to  localize  its  whereabouts  at  any  given 
time.  Global  localization  techniques  perform  better  than  local  ones  and  deal  with 
situations  in  which  the  robot  is  likely  to  experience  serious  positioning  errors  [32], 
Our  system  performs  localization  on  a  simplified  cognitive  version  of  the  map  as 
an  iterative  method  of  estimating  the  maximum  likelihood  of  the  correct  position 
of  the  robot  for  each  pose.  This  is  accomplished  with  the  use  of  the  Expectation 
Maximization  algorithm  [8]. 

1.1.3  Simultaneous  Localization  and  Mapping.  A  mapping  technique  which 
performs  both  mapping  and  localization  concurrently  generates  a  more  accurate  map. 
When  operating  together,  this  process  is  referred  to  as  Simultaneous  Localization 
and  Mapping  (SLAM)  or  Concurrent  Localization  and  Mapping  (CLM).  SLAM  is 
done  either  iteratively  or  once  at  the  end.  If  the  iterative  approach  is  used,  then 
it  localizes  while  the  robot  travels  and  maps.  If  the  batch  technique  is  used,  then 
mapping  and  motor  data  is  stored  while  the  robot  travels.  After  stopping,  the  robot 
compiles  the  data  into  a  complete  map  using  all  available  data.  The  later  method 
has  proved  the  most  successful  in  recent  work  using  the  Expectation  Maximization 
algorithm  [36]. 

Expectation  Maximization  SLAM  [36]  (explained  in  greater  detail  in  Section 
2.3),  perhaps  the  most  successful  SLAM  algorithm  available,  provides  the  inspiration 
for  the  localization  method  performed  in  this  thesis.  Expectation  Maximization  is 
a  general  algorithm  used  to  iteratively  calculate  the  maximum  likelihood  function 
of  some  probability  distribution  function.  The  term  ’’Expectation  Maximization” 
algorithm  was  coined  by  Dempster  et  al.  [8].  The  maximum  likelihood  function 
(taken  from  probability  theory)  calculates  the  maximized  expected  value  of  some 
probability  distributionFor  mapping,  this  probability  distribution  generally  includes 
obstacles  and  robot  locations  in  a  world  format. 
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1.1.4  Exploration  and  Planning.  Another  key  area  of  research  in  robotic 
mapping  is  exploration.  Does  the  robot  wander  randomly,  or  does  it  actively  explore 
areas  that  it  has  not  yet  experienced?  Complicating  the  process,  the  robot  should 
move  from  explored  to  unexplored  areas  in  such  a  way  that  it  minimizes  distance 
traveled  while  maximizing  new  mapped  areas.  To  actively  explore  the  region,  the 
robot  must  know  how  to  move  from  its  current  location  to  another  location  (Fig. 
1.2).  This  requires  that  the  robot  have  some  criterion  for  selecting  what  is  unexplored 
and  which  unexplored  region  to  travel  to.  This  requires  some  path  planning  on  part 
of  the  robot.  A  natural  extension  that  we  propose  to  our  technique  is  an  exploration 
feature  that  plans  using  simplified  polyline  data.  More  on  the  topic  is  discussed  in 
Section  2.1.2. 


Figure  1.2  The  robot  travels  in  the  heading  that  is  most  unexplored.  This  is  the 
shaded  area  on  the  left  and  bottom  of  the  figure. 


1.2  Research  Goals  and  Objectives 

Almost  every  robot  system  requires  some  sort  of  internal  representation  of  the 
world  that  the  robot  occupies.  In  today’s  military,  because  of  land  mines  and  other 
deadly  traps,  applications  needing  robots  to  scout  and  learn  environments  before 
humans  enter  them  are  apparent.  The  goals  of  this  research  are  to: 

•  map  a  region  quickly, 

•  map  an  area  as  accurately  as  possible, 
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•  provide  mapping  for  large  indoor  areas, 

•  and  represent  the  map  using  little  memory. 

The  objectives  used  to  accomplish  these  goals  are: 

1.  Modify  the  histogram  mapping  algorithm  to  provide  a  map  suitable  to  convert 
to  polylines. 

2.  Develop  a  new  edge  detection  algorithm  capable  of  quick  and  accurate  edge 
detection  with  the  histogram  map. 

3.  Create  a  method  to  represent  histogram  map  data  as  polylines. 

4.  Develop  a  topological  representation  of  the  polylines,  separating  spaces  or 
rooms  of  the  map  and  representing  them  as  nodes  in  a  graph. 

5.  Implement  and  test  a  new  algorithm  using  Expectation  Maximization  to  per¬ 
form  mapping  and  localization  corrections  on  the  simplified  topological  map. 

1.3  Approach 

The  system  is  developed  using  a  top  down  approach.  In  the  following  section, 
a  layered  abstraction  is  shown  of  the  complete  system  (Fig.  1.3).  At  the  top  is  the 
lowest  level  or  the  least  abstracted  level,  and  at  the  bottom  is  the  most  abstracted 
level.  That  is  what  is  meant  by  top  down ;  modules  are  designed  and  implemented 
starting  at  the  lowest  level  of  abstraction.  However,  sometimes  as  new  components 
are  added,  changes  are  needed  to  the  previous  components.  As  these  components 
are  modified  with  new  integrations  they  tend  to  evolve.  This  process  of  adding  and 
modifying  is  repeated  until  the  system  is  complete. 

1-4  Software  Design  Process 

Development  of  a  software  system  requires  the  use  of  a  structured  top  down 
design  approach  starting  with  a  problem  statement  and  ending  with  an  implemented 
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system.  The  overall  system  is  shown  in  (Fig.  1.3).  At  the  top  of  the  figure  some 
input  sensor  is  used,  whether  it  is  sonar,  stereo  camera,  laser  (not  shown),  or  some 
other  sensor  device.  If  a  camera  is  used,  data  from  the  camera  goes  first  to  an  image 
processing  subsystem  then  to  the  histogram  mapping  algorithm.  Using  sonars,  the 
data  is  sent  directly  to  the  mapping  algorithm.  The  next  layer  under  the  mapping 
itself  is  where  the  interesting  work  is  done.  In  this  layer,  the  system  converts  the  raw 
map  to  polylines,  topologically  represented.  Also,  localization  occurs  simultaneously 
with  the  topological  map  creation  at  this  stage.  If  path  planning  is  to  be  used  it 
would  be  placed  at  this  level  as  well. 


Figure  1.3  System  dataflow 


To  build  this  system  the  following  steps  are  repeated  for  each  module  of  the 
system  [22]: 
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Step  1  Define/analyze  domain  requirements  for  each  module. 

Step  2  Develop  a  general  solution  and  operational  design  specification  using  alge¬ 
braic  or  symbolic  notation. 

Step  3  Refine  the  solution  design  to  a  low-level  design  incorporating  additional  data 
structures  and  operations  required  to  create  an  algorithmic  design  template. 

Step  4  Map  the  algorithm  template  to  a  low  level  programming  language. 

1.5  Assumptions 

Robotic  mapping  has  many  elements  tied  to  computer  science  and  mathemat¬ 
ics.  To  reduce  the  scope  of  discussion,  the  following  assumptions  of  the  readers 
academic  background  are  made: 

1.  General  understanding  of  trigonometry  and  geometry 

2.  Basic  calculus 

3.  Comfortable  with  probability  and  set  theory 

4.  General  computer  science  background 

Image  manipulation  requires  an  understanding  of  trigonometry  and  geometry.  More¬ 
over,  sensor  fusion  and  mapping  in  general  is  steeped  in  calculus,  probability  and  set 
theory.  This  thesis  is  written  assuming  that  the  reader  is  familiar  with  these  types 
of  math  in  addition  to  elementary  computer  science  and  robots  in  general. 

1 . 6  Risks 

The  Pioneer  robot  in  which  the  mapping  system  is  built  has  limited  memory 
and  processor  capabilities.  Because  the  mapping  software  requires  very  close  to  real 
time  processing,  this  is  the  greatest  risk.  Moreover,  the  robot  is  controlled  with 
the  ARIA  robot  architecture  and  compatibility  issues  with  other  standard  library 
templates  is  also  a  key  risk. 
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1.7  Thesis  Outline 


In  Chapter  II,  background  information  related  to  robot  mapping  is  discussed 
along  with  some  key  areas  of  research  that  are  building  blocks  in  this  research. 
In  Section  2.2,  obstacle  mapping  is  explained  in  great  detail  with  an  emphasis  on 
Borenstein  and  Koren’s  technique  [4]  and  how  we  extend  it  in  our  work.  In  Section 
2.4,  some  cognitive  representations  of  the  robot  maps  are  discussed  again  focusing 
on  the  use  of  ASRs  for  topological  representation.  These  building  blocks  are  pieced 
together  (along  with  some  new  techniques)  into  a  design  and  implementation  in 
Chapter  III.  The  implementation  is  tested  and  results  are  analyzed  of  simulated  and 
real  world  mapping  in  Chapter  IV.  Finally,  future  work  and  extensions  are  discussed 
in  Chapter  V. 
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II.  Background  and  Related  Work 

The  goal  of  robot  mapping  is  to  build  a  useful  map  and  provide  a  solid  method  for 
the  robot  to  identify  and  avoid  obstacles  in  its  path,  and  if  revisiting  a  location, 
to  recognize  that  fact.  While  the  robot  explores  the  environment,  it  records  data 
readings  from  sensors  and  stores  that  data  for  this  purpose.  In  the  following  sections 
we  discuss  the  two  main  approaches  of  maintaining  and  representing  this  information. 
A  survey  of  several  robot  exploration  techniques  is  covered  in  Section  2.1.  In  Section 
2.2,  we  explore  obstacle  mapping  and  how  it  relates  to  this  work.  Finally,  Section  2.4 
details  cognitive  mapping  used  with  polylines  and  space/room  map  fragmentation. 

2. 1  Exploration 

Many  researchers  have  studied  exploration  strategies  along  with  their  research 
of  localization  and  mapping [21]  [11]  [20].  Exploration  is  a  pivotal  part  of  the  learning 
process;  if  we  don’t  explore,  how  do  we  learn  anything  new?  Benjamin  Kuipers  and 
Yung-Tai  Byun[21]  use  a  hill  climbing  strategy  to  perform  exploration. 

2.1.1  Kuipers  and  Byun  Exploration.  An  algorithm  proposed  by  Benjamin 
Kuipers  and  Yung-Tai  Byun  provides  a  good  method  of  exploration.  They  refer  to  a 
place  as  a  zero-dimensional  local  maximum,  and  a  path  followed  during  exploration 
is  defined  by  some  distinctiveness  criterion  that  is  sufficient  enough  to  specify  a  one¬ 
dimensional  set  of  points.  The  robot  follows  the  midline  of  a  corridor,  or  walks  along 
the  edge  of  a  large  space,  but  does  not  venture  into  the  interior  of  a  large  space. 
This  is  because  the  points  have  no  qualitatively  distinctive  characteristics,  at  least 
to  its  limited-range  sensors.  Paths  traveled  connecting  two  distinctive  places  are 
defined  in  terms  of  Local  Control  Strategies  (LCS).  A  LCS  is  a  strategy  for  what 
to  do  based  on  the  robots  current  location.  Once  a  place  has  been  identified,  the 
robot  selects  an  appropriate  local  control  strategy  for  moving  into  an  open  direction 
(that  is,  a  direction  that  has  no  obstruction).  While  following  a  path  chosen,  the 
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robot  continues  analyzing  its  input  for  signs  of  new  distinctive  features.  Once  the 
next  distinctive  place  is  identified,  the  path  connecting  the  two  places  is  defined  by 
the  LCS  [21].  This  method  is  deterministic  in  nature  but  offers  a  good  guide  to 
structuring  a  non- deterministic  method  of  accomplishing  the  same  task  [21]. 

2.1.2  Next  Best  View.  When  using  a  more  accurate  range  device  such  as  a 
laser  which  only  requires  one  scan,  the  robots  exploration  is  viewed  in  terms  of  views 
needed  for  scans.  After  performing  a  scan,  the  robot  determines  the  Next  Best  View 
(NBV)  which  will  provide  just  enough  overlap  with  the  current  scan  as  to  allow 
the  robot  to  merge  the  two  scans  together  properly  and  update  the  robots  dead 
reckoning  system  based  on  the  merged  data.  Hector  Gonzanos  and  John-Claude 
Latombe  make  use  of  this  strategy  to  perform  this  type  of  exploration  [11], 

Gonzanos  and  Latombe  [11]  define  a  safe  region  as  the  largest  regions  that 
are  safe  to  travel  in  given  sensor  readings  obtained  to  that  point,  while  providing 
the  minimum  amount  of  overlap  required  to  perform  alignment.  Exploration  and 
alignment  are  accomplished  simultaneously,  building  the  map  iteratively,  merging 
scans  from  each  safe  region  until  the  map  is  complete. 

This  technique  works  well  but  does  offer  some  limitations.  As  their  system 
only  uses  a  laser  which  scans  only  a  cross  section,  obstacles  can  be  missed  in  the 
scan.  Additionally,  errors  with  the  polyline  extraction  can  result  in  a  completely 
unusable  map  which  will  produce  incorrect  exploration. 

2.2  Occupancy  Grid 

Before  beginning  our  discussion  of  mapping,  it  is  appropriate  to  define  occu¬ 
pancy  grids  and  how  range  data  returns  a  positive  reading  for  a  specific  cell  within  the 
occupancy  grid.  One  can  think  of  the  proposed  map  as  an  image  to  be  created  in  a 
two  dimensional  matrix  denoted  C.  The  first  step  is  selecting  a  value  p  that  is  a  scaler 
used  to  determine  cell  size,  and  making  a  determination  of  the  world  size  to  represent 
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Cmn  where  m  is  the  max  distance  divided  by  p  expected  in  the  x  direction,  n  is  the 
max  distance  expected  to  travel  in  the  y  direction  divided  by  p.  All  calculations  are 
based  on  a  standard  cartesian  system.  It  is  important  to  understand  that  information 
from  the  sonars  is  polar,  that  is,  we  know  how  far  away  it  sees  an  object  and  at  what 
angle.  The  robot  has  an  orientation  h  :  h  —  {  —  180, —179, —178, ...,  178, 179, 180} 
relative  to  the  initial  orientation  of  the  robot  from  when  the  mapping  process  be¬ 
gan.  Turns  to  the  left  decrease  h  and  turns  to  the  right  increase  h.  Each  sonar  has 
an  angle  shi  :  i  =  {1,2,3 ,  ...number  of  sonars}  relative  to  the  robot.  The  global 
angle  of  each  sonar  is  then  ht  =  h  +  shi.  So  a  cartesian  point  for  sensor  i,  {x^yf)  is 
calculated  by: 


Ti  x  cos  (hi) 

Xi  =  - - — 

P 

r%  x  sin  (A) 


(2.1) 

(2.2) 


Cells  are  updated  for  each  reading  returned  by  the  sonars  using  an  update  function 
which  is  dependent  upon  the  specific  mapping  algorithm  used.  Update  functions  vary 
between  the  three  main  available  methods  of  mapping  which  are:  probabilistic  [26], 
belief  [27]  and  discrete  [4],  Generally,  the  most  accurate  is  the  belief  method  (using 
the  Dempster-Shafer  belief  theorem)  followed  by  the  probabilistic  approach.  Both 
probabilistic  mapping  methods  are  slower,  requiring  more  computation  than  discrete 
mapping  techniques.  In  Section  2.2.1  the  first  probabilistic  mapping  technique  known 
as  Bayes  mapping  [26]  is  discussed,  followed  by  the  mapping  technique  providing  a 
major  enhancement  known  as  the  Dempster-Shafer  method.  Finally,  in  Section  2.2.3, 
a  fast  discrete  mapping  technique  which  approximates  probability  values  calculated 
in  [26]  [27],  known  as  Histogram  in  Motion  Mapping  (HIMM)  [4],  is  explained  in 
great  detail.  HIMM,  designed  for  speed,  is  the  fastest  and  simplest  of  the  algorithms 
and  is  the  method  used  and  extended  in  this  thesis. 
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2.2.1  Bayes  Mapping.  One  of  the  most  straightforward  methods  of  rep¬ 
resenting  a  map  has  been  the  occupancy  grid  [26].  In  this  method,  the  world  is 
represented  as  a  two  dimensional  array  of  probability  information  about  the  occu¬ 
pancy  of  a  grid  cell.  Using  information  from  the  robot’s  range  sensors  and  pose,  an 
array  cell  is  updated  with  new  probability  information  after  each  sensing  action  [26] . 
The  Bayesian  method  sees  each  sonars  data  as  a  probability  distribution  within  a 
cone  of  error  (Fig.  2.1).  The  angle  of  error  for  the  sonar  is  (3.  Area  I  in  the  cone 
is  the  where  an  obstacle  is  thought  to  be.  Because  a  sonar  cannot  sense  through  an 
object,  area  II  is  assumed  unoccupied. 


Beyond  Sonar  Range 


Sonar 


Figure  2.1  Probabilistic  Sonar  Cone 
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The  Bayes  formula  (->  implies  negation): 


Pr(o|A  B)  =  PrQ4Kg)  Pr(o|B) 

Pr(-io|.4,  B)  Pr(4|-w,B)  Pr(^o|B)  1  '  1 

provides  a  combination  of  independent  data  A  and  B  as  single  data  item,  Pr(o|A,  B). 
o  is  the  observation,  A  is  the  current  data  item  and  B  the  prior  probability.  As 
the  sensors  create  readings  independently  of  each  other,  each  sensor  is  considered 
an  independent  data  source.  Moravec  [26]  uses  this  as  a  basis  to  derive  the  map 
combining  formula: 

Pr(o|.4,.B)  Pr(«|B)  Pr(o|4)  Pr(^o) 

Pr(^o|4,B)  Pr(-.o|B)  Pr(^o|4)  Pr(o)  '  [  '  1 

Let  Pr(r)  prior  to  a  reading  be  the  probability  that  the  next  reading  results  in 
measurement  r.  Denote  each  possible  range  Rt  (for  example,  if  the  sonars  have  a 
max  range  of  10m  and  our  cell  size  is  lrn  each  then  i  =  {0, 1,  2, ...,  10}).  To  find  the 
probability  for  each  R  use  [26]: 


Pr  (Ri) 


S(Ri 


x 


E  PrW 


l=o 


(2.5) 


where  S(Ri)  is  the  sum  of  the  products  at  Ri.  The  quantity  emiSS  is  the  small 
possibility  that  an  occupied  cell  will  not  be  detected.  Calculate  S(R)  using: 


S(r)  =  X  (a  “  emiss)-  (2.6) 

R 

More  specifically,  Moravec  defines  two  update  functions  that  approximate  the 
sonar  error,  one  for  cells  on  the  range  (this  is  region  1  of  (Fig.  2.1))  and  another  for 
all  cells  that  are  before  region  I,  denoted  region  II  [30].  Define  all  symbols  as  shown 
in  (Table  2.1). 

Putting  it  all  together  forms  the  equations  in  (Table  2.2)  [30]. 
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Table  2.1  Symbol  key  for  Bayesian  mapping  formulas. 


r 

Distance  or  range  returned  by  sensor  reading 

a 

Angle  of  the  sensor. 

R 

Maximum  distance  of  the  sensor. 

(3 

Viewable  range  of  sensor. 

MO 

Probabilistic  assumption  that  the  reading  is  not  always  correct. 

n 

Number  of  sensors. 

i 

Range  data  returned  by  sensor  i  :  i  —  1,  2,  3, ...,  n 

H 

Belief  of  occupation. 

Table  2.2  Bayesian  mapping  belief  functions 


Pr(Occupied) 

=>  Prfc|if) 

Pr  (Empty) 

=>  Pr(^hR) 

Region  I 

R—r  (3  — a 

R  2  0  x MO 

R—r  ,(3  —  ol 

1  R  2  0  xMO 

Region  II 

R—r  ,  (3  —  oc 

1  R  '  (3 

2 

R—r  ,  (3  —  ol 

R  '  (3 

2 

Table  2.2  gives  probabilities  Pr(y| Occupied)  and  Pr(y|-i Occupied)  for  regions  I 
and  II,  but  we  need  Pt  (Occupied^)  and  Pv(~^Occupied\0).  Apply  Bayes  formula  to 
get  [30]: 

p  Pr(y  |  Occupied)  Pr (Occupied) 

Pr  (^{Occupied)  Pr  (Occupied)  +  Pr(y|  Empty)  Pr  {Empty) 

The  Pr  (Occupied)  and  Pr  [Empty)  are  the  prior  probabilities  denoted  Pr  (H)  and 
Pr(-i H),  respectively.  Leading  to  the  update  rule  [30]: 


Pr(#|<ti,  ...,0 


Pr(<u,  <vn\H)  Pr (H) 

Pr(<u,  ...,9*1  H)  Pr (H)  +  Pr(?1, ..,  H)  Pr (-R) 


(2.8) 


=>  Pr(^  |ft) 


Pr(9|^)  Pr (H) 

Pr(9| H)  Pr (H)  +  Pr(ft|-. H)  Pr (-.if) 


(2.9) 
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Figure  (Fig.  2.2)  shows  a  three  dimensional  view  of  the  sonar  cone  as  a  probability 
density  function. 


Figure  2.2  A  three  dimensional  view  the  sonar  cone. 


2.2.2  Dempster- Shafer.  Probably  the  most  accurate  and  informative  map¬ 
ping  technique  without  localization  is  the  Dempster-Shafer  (DS)  method[30].  The 
DS  belief  theorem  allows  possibilities  to  represent  conflicting  evidence.  That  is, 

Proposition  {Occupied  A  Empty} 

=>■  Frame  of  discernment  { Occupied ,  Empty ,  Don't  Know} 

And  the  belief  function  must  satisfy: 

•  Bcl(0)  =  0,  (Define  0  as  the  empty  set.) 

•  Bcl(0)  =  1  =>■  Bel(//)+Bcl(-ii/)+  Bcl(0)  =  1.0,  (0  is  the  set  of  all  possible 
outcomes.) 

For  every  non-negative  integer  n  and  every  {Aj|l  =  1,  2,  3, ...,  n}  C  0: 

Bel(Ai)  >  ^  -lI+1Bel  (2.10) 

IC{A1)...,An}J^0  \i=I  ) 
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Simply  stated,  belief  functions  contributing  evidence  to  ©  can  be  combined  to  provide 
a  greater  belief.  Symbols  are  still  defined  by  (Table  2.1)  and  the  belief  function  is 
defined  as  shown  in  (Table  2.3)  [30]. 


Table  2.3  Dempster-Shafer  mapping  belief  functions 


m(Occupied) 

m  (Empty) 

m(Don’t  Know) 

Region  I 

R—r  ,  0  —  a 

R  j 0  x MO 

0.0 

R—r  0  —  oc 

1  R  2  13  x  MO 

Region  II 

0.0 

R—r  ,  0  —  ct 

R  '  0 

2 

R—r  0  —  cx 

1  R  '  0 

2 

The  update  function  is  then  [30]: 


m(occupied) 


'LAiABJ=OccuPiedm(Ai)m(Bj) 

1 


(2T1) 


m(Empty) 


YtAjAB^Empty  mjA^mjBj) 

1  -  EJ4iAB,=0m(A)m(Sj) 


(2.12) 


m(Don'tKnow) 


S AiABj  =Don'tKnow  m (Ai)m {Bj ) 


The  conflict  between  readings  is  removed  through  normalization  [30]: 


Con(Beli,  Bel2) 


log 


T,AiABj=eml(Ai)m2(Bj 


(2.13) 


(2.14) 


Borenstein  and  Koren  improvised  the  occupancy  grid  map  idea,  allowing  for 
faster  processing  and  making  it  possible  for  use  in  real  time  obstacle  avoidance  and 
robot  navigation  by  approximating  the  probability  values  [4], 
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2.2.3  Histogram  In  Motion  Mapping.  Borenstein  and  Koren’s  approach, 
Histogram  In  Motion  Mapping  (HIMM)[4],  represents  obstacle  data  in  a  two  dimen¬ 
sional  array  denoted  as  C.  When  a  sonar  indicates  a  reading  for  a  specific  cell  Cxy  in 
the  array,  Cxy  is  modified  so  that  Cxy  Cxy+ 3  and  all  its  adjacent  cells  (a;±l)(7/±l) 
are  incremented  by  .5.  All  cell  values  in  C  are  limited  so  that  0  <  C'xy  <  15.  Ad¬ 
ditionally,  cells  along  the  path  from  the  robot  center  point  (cr0, 2/o)  to  (x,y)  denoted 
as  Cij  are  decreased  by  1  (Fig.  2.3).  Notice  that  this  technique  assumes  a  higher 


Beyond  Sonar  Range 


Sonar 


Figure  2.3  Histogram  in  Motion  Mapping  Sonar  Cone 

belief  that  a  cell  is  occupied  than  not  occupied.  This  simple  method  of  building  a 
map  demands  little  processor  time  and  memory  space,  so  it  is  quickly  processed  by 
a  computer. 
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Borenstein  and  Koren  also  introduce  the  idea  of  using  a  polar  histogram  (Fig. 
2.4)  for  obstacle  avoidance[5]. 


Figure  2.4  Polar  Vector  Field  Histogram 

Within  a  predetermined  window  around  the  robot,  a  polar  sweep  is  made  in  5° 
increments.  The  angle  f3  of  the  current  point  (i,j)  with  respect  to  the  vehicle  center 
point  ( Xo,yo ),  is  determined  by, 

j3i  j  =  arctan  f  - — —  )  (2-15) 

\l-XoJ 

For  each  cell  (i,  j),  magnitude  is  calculated  by, 

(2.16) 

and  k  is  set  so  that  the  value  within  Ct]  is  scaled  where  the  closer  it  is  to  the  robot 
the  higher  the  value  it  is  assigned,  and  the  farther  away,  the  less  weight  it  is  given. 
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For  each  cone  0,  cells  are  summed  to  determine  a  magnitude  vector  k^. 


%  <—  T,  rriij  (2.17) 

ij 

Determination  of  which  cone  point  (i,j)  belongs  to  is  accomplished  by  finding  /3ij, 
where  (j>  —  1  <  <  (j)  indicates  point  (i,j)  in  the  cone  from  (0  —  1  ,</>].  The  robot 

travels  in  the  direction  with  the  largest  grouping  containing  lower  values  of  k. 

An  advantage  of  H1MM  is  its  ability  to  progressively  adapt  the  strength  of  an 
obstacle  avoidance  reaction  to  the  level  of  evidence  for  the  existence  of  an  obstacle, 
and  do  so  quickly.  By  using  this  technique  which  is  computationally  cheap,  we  free 
up  processor  time  for  use  in  the  cognitive  conversion  process  while  providing  obstacle 
avoidance.  Another  popular  approach  to  mapping,  known  as  Expectation  Maximiza¬ 
tion  (EM)  [36],  holds  probabilistic  information  about  the  world  and  constructs  the 
most  likely  map  from  it. 

2.3  Expectation  Maximization  for  Robotic  Mapping  and  Localization 

The  Expectation  Maximization  algorithm  or  the  concept  of  it  has  been  around 
since  the  late  1950’s.  However,  it  was  not  put  together  as  a  complete  algorithm  until 
1970  by  Baum  et  al.  [1].  Later  in  1977,  Dempster  et  al.  [8]  formalized  the  algorithm 
and  coined  the  term  Expectation  Maximization. 

EM  is  used  in  many  scientific  fields  including  medicine,  electrical  engineering, 
business,  and  computer  science.  Within  computer  science  EM  is  used  for  imaging, 
networking,  robotic  mapping  and  localization  and  many  other  applications.  Thrun, 
et  ah,  [36]  performed  the  seminal  work  that  extends  the  EM  algorithm  to  the  use 
of  mapping  and  localization  for  robotic  applications  using  landmarks.  To  use  the 
EM  algorithm  for  mapping  and  localization,  the  map  and  robot  path  is  formed  as 
a  maximum  likelihood  function.  This  function  is  then  maximized  to  determine  the 
most  likely  map  and  path  traveled. 
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The  Expectation  Maximization  algorithm  alternates  between  an  expectation 
step  (E-step)  and  a  maximization  step  (M-step)  [8].  In  the  E-step,  the  current 
map  is  not  changed  but  the  probability  distributions  are  calculated  for  past  and 
current  robot  locations.  In  the  M-step,  the  most  likely  map  is  computed  based  on 
the  estimation  result  gained  from  the  E-step  [36].  Alternating  both  steps,  the  robot 
simultaneously  improves  its  localization  and  its  map,  performing  hill-climbing  or 
gradient  assent  to  lead  to  a  local  maximum  in  likelihood  space.  The  probabilistic 
nature  of  the  estimation  algorithm  makes  it  considerably  robust  to  ambiguities  and 
noise,  both  in  the  odometry  and  in  perception.  Additionally,  it  enables  the  robot  to 
revise  past  location  estimates  with  new  sensor  data  [36]. 

The  Expectation  Maximization  method  used  in  [36]  is  effective  but  has  some 
weaknesses.  Even  constraining  the  algorithm  where  only  discrete  probabilities  are 
considered  with  a  spatial  resolution  of  1  meter  and  angular  resolution  of  5  degrees, 
the  processing  time  required  for  their  experiments  is  still  close  to,  2  hours.  Some 
methods  to  speed  up  processing  are  [36]: 

•  caching, 

•  symmetry  exploitation, 

•  coarse-grained  temporal  resolution, 

•  selective  computation,  and 

•  selective  memorization. 

Explanations  of  each  can  be  found  in  [36].  These  techniques  applied  in  his  experi¬ 
mentations  reduce  computation  time  by  approximately  2.98  x  108  [36]. 

2-4  Cognitive  Mapping 

Using  a  simple  occupancy  grid  presents  several  problems.  Of  these  problems, 
the  amount  of  memory  needed  to  represent  a  large  area  is  of  primary  concern.  For 
example,  if  the  grid  resolution  is  10  centimeter  by  10  centimeter  cells,  mapping  a 
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100  x  100  meter  area  requires  (100  x  100)  x  (100  x  100)  x  16 bytes  =  1.53gb  of  memory 
space.  Performing  localization  on  a  dataset  this  large  is  computationally  difficult 
making  it  important  for  us  to  represent  the  same  information  in  a  more  memory 
friendly  fashion. 

Cognitive  mapping  with  the  use  of  ASRs  is  a  topological  mapping  approach. 
That  is,  the  world  is  represented  more  from  the  perspective  of  the  robot  and  in  some 
cases  maintains  very  little  metric  information  about  the  world.  Instead,  the  map  is 
created  with  consideration  to  where  the  ASRs  are  with  respect  to  each  other  and 
the  robot. 

2-4-1  Absolute  Space  Representation.  An  Absolute  Space  Representation 
(ASR)  [13],  is  a  cognitive  mapping  technique  used  to  build  models  of  rooms  or  spaces 
visited.  Absolute  space  comes  from  the  idea  that  the  representation  for  each  space 
should  be  independent  of  all  other  spaces.  Also,  humans  view  the  world  cognitively 
and  so  should  our  robots  for  ease  of  relation. 

Rooms/nodes  are  separated  by  access  points/edges,  that  in  the  graph  are  rep¬ 
resented  as  edges  connecting  nodes.  This  technique  is  explained  by  Hill,  Han,  and 
Lent  [13].  While  the  idea  of  the  ASR  [13]  has  a  foundation  with  three  dimensional 
image  based  localization  and  mapping  (much  like  Video  SLAM  [32],  using  video  to 
extrapolate  range  data  and/or  landmark  data),  it  is  also  applicable  with  respect  to 
indoor  mapping  using  ultrasonic  sonars  as  the  range  device  [20].  Figure  2.5  shows  a 
standard  Cartesian  map  translated  to  an  ASR.  Notice  the  rooms  become  nodes  and 
the  exits  become  edges  connecting  the  nodes. 

The  use  of  ASRs  becomes  more  necessary  as  the  size  of  the  mapped  environ¬ 
ment  increases.  By  placing  non-current  ASR  data  in  persistent  storage,  the  robot 
must  only  hold  a  limited  amount  of  information  in  main  memory  at  any  given  time. 
Additionally,  when  the  robot  uses  the  cognitive  map  at  a  later  time,  it  need  only 
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Cartesian 


A  SR 


Figure  2.5  Two  Room  ASR  Example 

know  which  ASR  it  currently  occupies  and  have  the  data  for  that  ASR  and  its 
neighboring  ASRs  loaded  into  memory. 

Jeffries  and  Yeap  [17]  expand  the  theory  of  cognitive  maps  which  at  its  core 
has  the  notion  of  an  autonomous  agent  building  a  map  in  memory,  i.e.  a  map  in 
the  head  termed  a  cognitive  map,  for  the  places  it  visits.  The  agent  first  develops 
a  representation  for  each  space  visited  [39]  [38].  The  space  occupied  by  the  robot, 
termed  the  local  space,  is  defined  as  the  region  which  the  robot  perceives  enclosing 
it  (a  room;  hall,  etc).  The  robot’s  cognitive  map  grows  as  the  representation  of  each 
local  space  it  visits  is  added  to  a  topological  network  or  graph  of  ASRs. 

Jeffries  and  Yeap  [17]  initially  test  this  approach  with  a  simulation  of  an  agent 
traveling  in  a  complex  2D  representation  of  a  hospital  environment.  Data  produced 
by  this  simulation  includes  a  comprehensive  description  of  the  underlying  theory  and 
algorithms  employed  in  its  implementation.  This  theory  is  applied  to  the  problem  of 
an  autonomous  mobile  robot  equipped  with  sonar  sensors,  building  a  map  from  its 
experience  of  the  places  it  has  visited  [17].  In  this  case  the  cognitive  map  consists 
of  a  local  space  representation  for  each  local  space  visited  with  connections  to  other 
rooms  that  have  been  experienced  as  neighbors.  All  local  space  representations  have 
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their  own  local  coordinate  systems  and  are  independent  of  all  others.  Onr  approach 
builds  on  this  method  by  applying  the  use  of  polylines  to  the  representation  of  the 
ASR.  This  enhances  our  ability  to  use  the  ASR  for  navigation  and  localization. 

2-4-2  Topological  Mapping.  It  is  important  to  note  that  cognitive  map¬ 
ping  is  a  subset  of  a  more  general  type  of  mapping  known  as  topological  mapping. 
Topological  mapping,  in  the  truest  sense,  is  mapping  where  everything  in  the  map 
is  represented  with  a  position  reference  based  on  something  else.  In  a  typical  indoor 
environment  a  hall  can  be  an  edge  connecting  two  rooms,  and  the  rooms  can  be  ver¬ 
tices  on  a  graph.  Jensfelt  [19]  provides  a  good  survey  of  some  topological  mapping 
schemes.  The  advantage  of  this  approach  is  that  they  can  be  constructed  with¬ 
out  knowing  the  exact  geographical  relation  between  nodes  as  they  are  an  abstract 
construct  [19]. 

2-4-3  Polyline  Simplification.  Most  implementations  of  robot  mapping 
represent  map  data  as  some  form  of  occupancy  grid  or  topological  network.  Our 
approach  merges  these  two  ideas.  We  maintain  world  data  by  transforming  a  tem¬ 
porary  occupancy  grid  to  a  set  of  lines  and  structuring  those  lines  in  a  topological 
network.  The  size  of  the  data  structure  is  significantly  smaller  using  polylines  in 
place  of  an  occupancy  grid,  and  using  a  cognitive  map  for  path  planning  and  lo¬ 
calization  becomes  much  faster  as  the  search  space  is  reduced.  Using  a  polygonal 
representation  with  sonars  has  been  previously  considered  infeasible,  as  the  amount 
of  noise  generated  by  the  sonars  complicates  the  process  greatly  [11].  Latombe  and 
Banos  [11]  use  polylines  with  the  help  of  a  laser  range  finder,  instead  of  sonars.  The 
vehicle  moves  from  point  to  point  and  performs  a  polar  sweep  at  each  point.  Each 
new  polar  sweep  is  added  to  the  current  model.  Lines  are  extracted  from  the  raw 
range  data  using  a  polar  line  fitting  algorithm.  Using  sonars  necessitates  the  need 
for  developing  a  new  method  of  fitting  range  data  to  lines.  We  extend  their  system 
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by  developing  a  new  data  fitting  method  and  storing  the  data  in  a  cognitive  map  for 
use  in  later  localization  and  navigation. 

2-4-4  Douglas- Peucker.  It  is  necessary  after  extracting  vertices  from  range 
data,  to  simplify  and  reduce  vertices  that  are  not  needed  to  represent  the  map.  A 
popular  method  in  the  mathematics  community  known  as  the  Douglas-Peucker  line 
simplification  method  [9]  works  well  to  reduce  unneeded  vertices  in  polylines.  The 
Douglas-Peucker  line  simplification  algorithm  [9]  uses  the  closeness  of  a  vertex  to 
an  edge  segment  as  judgment  criterion  for  elimination.  The  algorithm  starts  with  a 
crude  guess  of  a  simplified  polyline,  the  single  edge  joining  the  first  and  last  vertices 
of  the  polyline.  Remaining  vertices  are  tested  for  closeness  to  that  edge.  If  these 
vertices  are  further  than  a  specified  tolerance  from  the  edge,  then  the  furthest  vertex 
from  it  is  added  to  the  simplification.  This  process  creates  a  new  guess  for  the 
simplified  polyline.  Applying  this  process  recursively  for  each  edge  until  all  vertices 
of  the  original  polyline  are  within  tolerance  of  the  simplification  yields  the  final 
simplified  set  of  edges  (Fig.  2.6). 


Figure  2.6  Steps  in  the  Duaglus  Peuker  Algorithm 
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2-4-5  Edge  Detection.  Extracting  points  from  the  grid  is  a  close  problem 
to  the  more  common  edge  detection  problem  in  image  processing.  Consequently, 
there  are  many  algorithms  available  which  work  well  to  accomplish  this  task  when 
the  edges  (in  our  case  obstacle  boundaries)  are  well  defined.  Sonars  produce  noisy 
edges  and  so  most  of  these  algorithms  are  not  efficient  to  accomplish  this  task.  One 
algorithm  called  the  Hough  Transform  [14]  also  works  well  when  the  edges  are  not 
well  defined  and  is  used  often  in  robotic  mapping  for  edge  extraction  [18]  [32],  A 
problem  with  using  the  Hough  Transform  is  the  amount  of  computation  required 
hireling  edges.  This  method  is  general  in  nature  and  not  specific  to  robot  maps. 
With  a  map,  more  information  is  known  about  where  edges  should  or  should  not  be 
based  on  the  path  of  the  robot  and  areas  beyond  the  range  of  the  robot’s  sensors. 
This  information  speeds  up  the  location  and  determination  of  edges  in  the  map. 

2. 5  Localization 

Monte  Carlo  Localization  (MCL)  provides  a  solution  to  the  global  localization 
and  kidnapped  robot  problem  in  a  highly  robust  and  efficient  way.  MCL  accommo¬ 
dates  arbitrary  noise  distributions  and  therefore  avoids  a  need  to  extract  features 
from  data  originating  from  whatever  sensors  that  may  be  used.  These  sensors  can 
be  sonars,  lasers,  cameras,  or  any  other  type  of  range  detection  device.  We  denote 
a  belief  to  be  the  agent’s  certainty  when  it  detects  an  object,  as  how  sure  it  is  of 
where  that  object  is,  and  if  it  is  even  an  object.  Representing  the  belief  is  a  set  of 
samples  o,  drawn  according  to  the  posterior  distribution  over  robot  poses  a  and  is 
the  key  idea  of  MCL.  The  set  of  particles  is  represented  as  random  variable  x.  The 
MCL  performs  a  recursive  update  function  to  ford  the  recursive  function 

Bel(xt)  =  p(o\xt)  J p(xt\xt-i,at-i)Bel(xt-i)dxt-i  (2-18) 

where  t  is  time. The  MCL  recursive  update  function  is  performed  as  a  three  stage 
process[37]: 
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1.  Sample  xt-\  from  the  sample  set.  Distribute  xt  according  to  the  belief  distri¬ 
bution  Bel(xt- 1). 

2.  The  pair  (xt,xt-i)  is  distributed  according  to  the  product  distribution 

qt  =  p(xt\xt-i,  at- 1)  x  Bel(xt-i)  (2.19) 

3.  Correct  the  mismatch  between  distribution  qt  and  the  desired  distribution 
(Equation  2.18).  The  deference  is  accounted  for  using  the  following  impor¬ 
tance  factor: 


p(ot\xt)p(xt\xt-i,at-i)  x  Bel(xt-i) 
p(xt\xt-i,  at-i)  x  Bel(xt-i) 


p(ot\xt) 


(2.20) 


So,  rather  than  approximating  posterior  distributions  in  parametric  form,  as  is  the 
case  for  Kalman  filter  and  Markov  localization  algorithms,  MCL  represents  the  pos¬ 
teriors  by  a  random  collection  of  weighted  samples  that  approximate  the  desired 
distribution  X.  Most  work  estimating  the  state  recursively,  using  samples,  is  very 
recent.  Computer  vision  researchers  have  proposed  the  same  algorithm  as  the  con¬ 
densation  algorithm  [37]. 

The  MCL  method  based  on  the  condensation  algorithm  is  a  vision-based 
Bayesian  filtering  method  that  uses  a  sampling-based  density  representation  and 
can  represent  multi-modal  probability  distributions  [7].  Using  a  visual  map  of  the 
ceiling  obtained  by  mosaicing,  it  localizes  the  robot  globally  using  a  scalar  brightness 
measurement.  Jensfelt,  et  al.  [19]  introduced  modifications  allowing  greater  efficiency 
in  large  symmetric  environments.  Sensor  information  (sonar,  laser  or  brightness  mea¬ 
surements)  provides  low  feature  specificity  requiring  that  they  are  probabilistic  and 
that  the  robot  move  around  while  probabilities  gradually  converge  toward  a  localized 
peak  [32], 
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Several  algorithms  have  been  developed  in  detail  to  perform  localization.  Neira  [28] 
describes  a  recursive  branch  and  bound  algorithm  called  Geometric  Constraints 
Branch  and  Bound  (GCBB),  which  computes  the  best  hypothesis  H  from  the  avail¬ 
able  measurements  and  the  features  of  the  stochastic  map,  using  unary  (one)  and 
binary  (two)  location  independent  geometric  constraints.  The  algorithm  starts  with 
an  empty  hypothesis  and  proceeds  in  a  depth-first  branch  and  bound  manner.  At  the 
leaf-level  solution  space  of  the  interpretation  tree,  the  algorithm  checks  whether  this 
leaf  is  a  hypothesis  having  more  consistent  pairings  than  the  current  best.  Satisfying 
binary  geometric  constraints  does  not  guarantee  that  a  hypothesis  is  globally  consis¬ 
tent  [12],  the  vehicle  location  is  then  estimated  and  a  joint  compatibility  test  [28]  is 
used  to  verify  global  consistency  of  the  matching  hypothesis.  The  algorithm  performs 
a  bounded  search,  evaluating  the  potential  benefit  of  considering  the  star-branch  be¬ 
fore  it  recurses.  Branching  can  be  done  by  pre-ordering  the  sensor  measurements  so 
that  the  most  precise  are  considered  first.  We  can  pre-compute  unary  and  binary 
constraints  before  recursion  starts  because  they  are  both  location  independent  [29]. 

An  alternative  algorithm,  originally  developed  for  geometric  object  recogni¬ 
tion  by  Bollcs,  et  al.  [2]  and  Faugeras,  et  al.  [10],  follows  a  hypothesis  generation- 
verification  scheme.  Location  independent  unary  and  binary  geometric  constraints 
are  useful  to  generate  candidate  hypotheses,  just  as  in  GCBB.  However,  as  soon  as 
the  number  of  pairings  is  adequate  to  determine  the  vehicle  location  (when  we  have 
two  points  and  two  non-parallel  segments),  the  verification  of  the  hypotheses  takes 
place,  predicting  the  location  of  map  features  and  also  searching  for  pairings  with 
the  remaining  measurements. 
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III.  Design 

Development  of  a  software  system  requires  the  use  of  a  structured  top  down  de¬ 
sign  approach  starting  with  a  problem  statement  and  ending  with  an  implemented 
system.  Robotic  mapping  systems  afford  the  opportunity  for  modular,  sequential 
development.  First,  the  robot  must  travel  before  it  can  map,  therefore  the  guidance 
and  control  is  accomplished  first.  Next,  a  mapping  algorithm  is  picked  to  map  as 
the  robot  moves.  After  the  robot  wandering  and  mapping  algorithms  are  completed, 
the  efficiency  of  the  stored  information  is  examined  and  optimized.  The  cognitive 
conversion  algorithm  is  developed  before  moving  on  to  localization.  Localization, 
typically  the  most  difficult  step  in  performing  Simultaneous  Localization  And  Map¬ 
ping  (SLAM),  is  processor  intensive,  therefore  it  is  important  for  the  robot  to  localize 
using  the  simplified  cognitive  map. 

The  first  floor  of  the  Air  Force  Institute  of  Technology  building  (Fig.  3.1) 
provides  an  ideal  testing  environment  for  the  robot  mapping  system  and  is  where 
we  perform  most  real  world  testing.  Another  office  building  is  used  for  the  small 
cluttered  environment  test  however  no  blueprint  is  available  for  that  office. 

3. 1  Implementation 

The  system  developed  in  this  thesis  is  comprised  of  several  distinct  algorithms. 
At  the  highest  level,  the  system  consists  of  the  robot  traveling  around  its  environment 
creating  a  Modified  Histogram  in  Motion  Mapping  (MHIMM)  map.  Each  time  the 
robot  detects  that  it  leaves  a  room  (enclosed  area),  it  creates  a  complete  polygonal 
map  of  the  previous  room  and  saves  that  map  to  the  hard  drive  as  an  Absolute 
Space  Representation  (ASR)  represented  by  polylines.  A  new  map  is  started  and 
the  process  begins  again  in  the  new  room.  The  mapping  is  accomplished  real-time. 
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Creating  a  map  of  a  room  is  a  multistage  process.  It  begins  by  cleaning  the 
HIMM  map  (Fig.  3.2a)  (50  x  50  =  2500  integer  matrix)  leaving  walls  represented  by 
no  more  than  one  cell  thick  lines  in  the  grid  (Fig.  3.2b)  (50x50  integer  matrix).  Next, 
an  extraction  algorithm  is  called  to  create  an  ordered  list  of  vertices  (250  x  2  =  500 
integers)  using  a  nearest  neighbor,  follow-and-remove  process.  Using  the  new  list  of 
vertices,  a  final  representation  (Fig.  3.2c)  (9  x  2  =  18  integers)  is  generated  using 
vertex  and  edge  reduction  algorithms.  Exit  and  entry  locations  are  stored  for  later 
use  as  pivot  points  for  rotational  adjustments  to  compensate  for  localization  errors. 
Finally,  the  robot  combines  all  the  ASRs  to  create  a  human  readable  master  map. 


Figure  3.2  Complete  polyline  process.  The  raw  map  is  shown  in  (a),  followed  by 
the  cleaned  map  (b).  Finally  (c)  is  the  simplified  fitted  polyline. 


To  create  this  map  the  robot  performs  a  two  staged  process,  first,  it  fragments 
the  map  so  that  each  discrete  area  or  room  visited  can  be  mapped  independently. 
Second,  it  converts  the  occupancy  grid  to  a  series  of  polylines  that  are  then  stored 
in  a  spatial  representation. 


3.1.1  Absolute  Space  Representation  (ASR)  Construction.  When  the  robot 
completes  the  mapping  of  one  room/space  it  constructs  an  ASR  from  the  MHIMM 
map.  This  construction  involves  creating  the  raw  MHIMM  map,  cleaning  it,  and 
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converting  it  to  a  cognitive  map.  Finally,  the  robot  adds  the  map  to  the  ASR 
container.  We  break  this  process  into  the  sequence; 

1.  cleaning  the  raw  HIMM  map  (Fig.  3.3a)  to  (Fig.  3.3b), 

2.  creating  an  ordered  list  of  vertices  by  extracting  points  (Fig.  3.3c), 

3.  eliminating  points  that  are  close  together  (Fig.  3.3d), 

4.  reducing  the  number  of  edges  (Fig.  3.3d)  to  (Fig.  3.3e), 

5.  fitting  the  remaining  edges  to  a  line  with  linear  regression  model  (Fig.  3.3e) 
to  (Fig.  3.3f)  to  (Fig.  3.3g), 

6.  calculating  a  reference  point  used  later  when  all  the  ASRs  are  put  back  together 
(Fig.  3.3h), 

7.  and  finally  calculating  the  localization  drift  and  adding  the  new  ASR  to  the 
ASR  container  (Fig.  3.3h)  to  (Fig.  3.3i). 

3.1.2  ASR  Boundary  Detection.  Before  mapping  an  ASR,  the  algorithm 
must  be  able  to  detect  the  beginning  of  a  new  ASR  and  the  end  of  the  previous  ASR. 
The  traversal  from  one  room  to  another  can  be  thought  of  as  a  series  of  states.  In 
a  room,  the  robot  typically  will  be  close  to  no  walls,  one  wall,  or  two  walls  that  are 
orthogonal  to  each  other.  Passing  from  one  room  to  another,  the  robot  passing  an 
access  point  will  detect  obstacles  close  to  it,  one  on  each  side  of  the  robot.  All  these 
possibilities  present  the  following: 

•  The  robot  is  in  a  room. 

•  The  robot  is  at  an  access  point. 

The  following  events  are  used  to  transition  among  states  (->  is  the  negation  symbol, 
A  represents  and,  and  V  represents  or.): 

•  An  obstacle  is  close  to  the  left  of  the  robot,  denoted  (L). 
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Figure  3.3  Steps  in  the  ASR  creation  process.  These  steps  involve  cleaning  the  raw 
HIMM  map  (a)  which  yields  (b),  then  points  are  extracted  creating  an 
ordered  list  of  vertices  (c),  then  simplified  (d)  and  (e).  These  points 
are  fit  to  a  line  with  a  linear  regression  model  (f)  and  (g).  Finally,  the 
ASR  is  added  to  the  map  (h)  and  localized  (i). 
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•  An  obstacle  is  not  close  to  the  left  of  the  robot,  denoted  (-T). 

•  An  obstacle  is  close  to  the  right  of  the  robot,  denoted  (R). 

•  An  obstacle  is  not  close  to  the  right  of  the  robot,  denoted  (_|R). 

If  the  robot  travels  from  a  room  to  an  access,  as  it  enters  a  new  room  an  exit 
is  detected  at  the  access  point.  This  is  shown  as  a  state  machine  in  (Fig.  3.4). 
Hallways  are  not  considered  in  this  algorithm.  In  later  testing  when  hallways  are 
mapped  ASRs  are  constructed  based  on  the  distance  the  robot  travels;  a  new  ASR 
is  created  every  4.5  meters. 


->  L  v ->R  v  “'R,  Exit  Detected 


3.1.3  Building  a  Mapping  Algorithm.  Section  2.2  gives  the  definitions 
shown  in  (Table  3.1)  that  are  used  in  this  section  to  develop  a  general  algorithm  for 
grid  mapping. 
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Table  3.1  Symbol  key  for  the  general  occupancy  grid  mapping  algorithm. 


Symbol 

Definition 

P 

Scalar  used  to  determine  cell  size. 

C 

2  dimensional  matrix  that  represents  the  world. 

m 

Max  distance  divided  by  p  expected  in  the  x  direction. 

n 

Max  distance  expected  to  travel  in  the  y  direction  divided  by  p. 

h 

Robots  direction. 

shi 

Each  sonar’s  angle  relative  to  the  robot. 
i  =  {1,  2,  3,  ...number  of  sonars} 

hi 

The  global  angle  of  each  sonar,  /q  =  h  +  shi. 

Sensors  are  not  at  the  center  of  the  robot  and  the  mapping  requires  the  location 
of  obstacles  relative  to  the  center  of  the  robot.  Each  sonar  has  an  x  offset  and  y  offset 
from  the  robot  center  point,  denoted  soxi  and  soyi  respectively.  This  offset  must 
be  transformed  from  the  robots  local  coordinate  system  to  the  global  coordinate 
system.  This  gives  us  a  modification  to  (Equation  2.1): 


n  x  cos(hj)  +xoffseti 

(3.1) 

p 

_  Vi  x  sin(hj)  +y0ffseti 

Vi 

(3.2) 

P 

As  expressed  in  [31]  the  relationship  of  a  point  (x,  y)  and  its  transformed  point  ( x y') 
given  a  circular  rotation  of  9  can  be  expressed  as: 


cos($) 

sin(6)) 


—  sin(0) 
cos(6>) 


(a;  cos  (9)  —  ysin(9) 
xsin(9)  +  ycos(9) 


(3.3) 


and  our  offset  for  each  sonar  is: 


Xoffseti  =  x'  =  soXi  cos (h)  -  soy^  sin (h)  (3.4) 

Uoffseti  =  y'  =  SQXi  sin (h)  +  soy j  cos (h)  (3.5) 
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substituting  this  into  (Equation  3.1)  and  (Equation  3.2)  and  adding  the  offset  for 
the  robots  current  pose  (x0,y0)  gives: 


Xi  = 


r'i  x  cos  (hi)  +  ( soxi  cos  (h) 


Vi  = 


P 

ri  x  sin  (hi)  +  (soxjsin(h) 


P 


soyi  sin(/r))  +  xq 


soyi  cos(h))  +  y0 


(3.6) 

(3.7) 


Figure  3.5  shows  a  transformation  of  a  robot  in  a  global  map  C  where  a  sonar 
detects  an  obstacle  at  point  p  that  is  transformed  to  point  p'  to  adjust  for  the 
misalignment  in  the  sonars  x  and  y  offsets  caused  by  the  difference  in  the  robots 
heading  and  the  world  frame. 


Figure  3.5  Local  coordinate  system  of  a  robot  in  a  global  map. 


Mapping  is  then  a  matter  of  cycling  through  all  the  sonars  for  each  sonar  scan 
and  updating  C,  the  map  (Fig.  A.l). 

3.1.4  Modified  Histogram  In  Motion  Mapping  Algorithm.  The  basis  of  the 
mapping  algorithm  is  the  underlying  modified  version  of  the  Histogram  In  Motion 
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Mapping  (HIMM)[4],  Following  is  the  development  of  the  HIMM  algorithm  and 
from  it,  modifications  needed  for  this  system. 

From  Section  2.2.3,  obstacle  data  in  a  histogram  is  represented  in  a  two  di¬ 
mensional  array  denoted  C .  The  active  spot  of  a  sonar  reading  (x,  y ),  C'  <—  Cxy  + 1 
and  all  its  surrounding  cells  (x±  l)(r/±  1)  are  incremented  by  .5.  All  cell  values  in  C 
are  limited  so  that  0  <  Cxy  <  15.  Additionally,  cells  along  the  path  from  the  robot 
center  point  (x0,y0)  to  (x,y)  denoted  as  Cij  are  decreased  by  1  (Fig.  2.3).  Moreover, 
the  further  away  an  obstacle  is  detected  by  a  sonar,  the  less  accurate  that  reading 
is  as  it  moves  further  out  in  the  sonar  cone  (Fig.  2.3).  Because  sonar  data  degrades 
with  distance,  only  points  within  a  window  {w  :  x0  —  w  to  x0  +  w  and  y0  —  w  to 
y0  +  w}  around  the  robot  are  considered.  Remember  that  each  sonar  has  an  x  offset 
and  y  offset  from  the  robot  center  point,  denoted  soxi  and  soyi  respectively.  Range 
data  returned  from  the  Pioneer  robots’  sonars  rx  are  in  millimeters  and  setting  <p  to 
10cm  works  well.  Inserting  the  HIMM  technique  for  the  update  function  of  the  occu¬ 
pancy  grid  mapping  algorithm  in  (Appendix  A,  Fig.  A.l)  results  with  the  algorithm 
shown  in  (Appendix  A,  Fig.  A. 2). 

The  need  to  make  more  defined  inside  edges  and  information  about  the  robot 
path  prompts  a  few  small  modifications  leading  to  our  Modified  Histogram  In  Motion 
Mapping  (MHIMM)  algorithm.  These  modifications  are: 

1.  For  all  updates  the  vehicle  center  point  (x0,  yo)  is  set  to  —100  to  mark  the  path 
of  the  robot.  Later  the  robot  will  need  this  information  to  clean  the  map. 

2.  Empty  regions  are  permitted  to  drop  below  0  to  —3.  For  all  x  and  y,  —3  < 
Cxy  <  15.  This  distinguishes  explored  from  unexplored  areas. 

3.  If  the  sonar  range  is  greater  then  w  then  all  cells  in  the  direction  of  that  sonar 
to  the  window  boundary  are  decreased  by  1.  This  modification  Liters  out  close 
proximity  noise  considerably. 

The  resulting  algorithm  is  shown  in  (Appendix  A,  Fig.  A. 3). 


3-9 


3.1.5  Clean  Map  Algorithm.  From  the  MHIMM  built  occupancy  grid 
map,  our  cleanup  algorithm  incorporates  some  modifications  of  the  original  mapping 
technique  provided  [4],  In  their  article,  empty  cells  empty (Cxy)  are  decreased  by  - 
1  but  not  allowed  to  drop  below  0.  To  increase  the  difference  between  occupied 
and  unoccupied  cells,  the  lower  bound  of  C  is  decreased  so  that  empty (Cxy)  >  —3 
allowing  us  to  distinguish  between  an  unexplored  region  and  an  empty  region.  More 
importantly,  spaces  occupied  by  the  robot  itself  are  marked  as  robotpath  (-100)  for  use 
in  the  map  cleaning  algorithm  (the  update  function  is  prevented  from  modifying  this 
value).  This  is  shown  in  (Fig.  3.6a)  and  the  modified  map  demonstrates  a  cleanly 
mapped  region  inside  the  empty  space  and  messy  area  in  the  unexplored  region  (Fig. 
3.6b). 


(a)  (b) 

Figure  3.6  A  HIMM  before  (a)  and  after  (b)  applying  the  HIMM  cleaning  algo¬ 
rithm.  The  line  inside  the  room  represents  the  path  of  the  robot. 

The  cleaning  process  is  straight  forward;  any  cell  deemed  empty  or  occupied 
is  checked  to  see  if  a  ray  from  it  to  the  path  of  the  robot  is  possible.  If  the  ray 
traverses  occupied  cells  then  it  is  set  to  an  unexplored  or  unknown  state  (Fig.  3.7). 
We  represent  the  map  to  be  cleaned  as  a  matrix  H  where  Hx  y  is  a  specific  position  of 
the  map.  The  final  filtered  map  is  H' .  Let  maxdist  be  the  maximum  effective  range  of 
the  sonars.  Part  of  our  algorithm  requires  creating  a  ray  from  every  point  (x,  y)  where 
HXjy  >  0  to  x±maxdist ,  yMmaXdist ■  All  line  functions  use  Bresenham’s  line  algorithm 
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Figure  3.7  Any  occupied  cell  not  able  to  draw  a  ray  to  the  path  that  the  robot 
traveled  without  going  through  another  occupied  cell  or  it  exceeds  the 
max  effective  distance  of  a  sonar,  it  is  discounted  as  noise. 


[6]  for  efficiency.  To  represent  the  line  algorithm,  we  define  T(pa,pb,  {start,  next}) 
which  returns  points  along  the  line  from  pa  to  pb.  Finally,  e  is  a  threshold  value 
where  HXjV  >  e  is  considered  occupied.  The  full  algorithm  is  shown  in  (Fig.  3.8). 


3.1.6  Point  Extraction  Algorithm.  One  of  the  big  challenges  is  taking  point 
data  from  the  two  dimensional  array  and  turning  it  into  a  list  of  organized  vertices, 
V.  The  points  must  be  retrieved  in  such  a  fashion  that  they  form  organized  polylines, 
the  point  on  the  map  that  the  edge  extraction  starts  being  arbitrary.  Simply  adding 
vertices  to  a  list  and  treating  them  as  a  polyline  results  in  a  jumble  of  seemingly 
random  lines.  A  point  in  H  is  only  added  to  the  vertex  list  V  if  Hxy  >  e.  Consecutive 
vertices  creates  segments  that  start  at  point  ps  and  end  at  pe. 

In  general,  the  algorithm  retrieves  points  by  following  occupied  cells,  adding 
the  point  to  V  and  removing  it  from  H  until  \H\  =  0.  This  algorithm  is  detailed  in 
(Fig.  3.9). 

While  retrieving  points  on  the  map,  it  is  important  to  identify  where  one 
polyline  ends  and  another  begins.  To  do  this,  a  threshold  variable  7,  is  defined  as 
the  maximum  allowable  distance  between  two  cells  containing  no  occupied  spaces. 
If  the  distance  is  greater  then  7,  a  break  is  inserted  denoting  the  end  of  a  polyline. 


3.1.7  Edge  Simplification  Algorithm.  Before  we  perform  any  edge  simplifi¬ 
cation,  it  is  important  to  reduce  unnecessary  vertices  first.  The  process  of  simplifying 
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CleanMap(iZ) 

For  (every  point  (x,y)  of  H) 

If (Hx,y  >  e) 

Pa  <-  {x,  y) 
found  <—  false 

For  (- maxdist  to  +maxdist  as  j) 
For  (- maxdist  to  +maxdist  as  i) 
Pb  <-  ((x  +  i),(y  +  j)) 

Pt  <-  r (pa,pb,  start) 

While  (pt  ±  pb) 

pt  <-  r (pt,pb,next) 

I  f  ('  l  f  i>  e  obotpaj}: ) 

found  true 
H'  —  Hv 

Pa  Pa 

Break  While  loop 

If (HP.  >  e) 

Break  While  loop 
If  ( found, )  Break  For  loop 
If  (found)  Break  For  loop 

End  For 

H  <- H' 


Figure  3.8  Algorithm  to  clean  a  modified  HIMM  map 


the  list  of  vertices  V  extracted  from  the  map  begins  by  eliminating  clusters  of  closely 
grouped  points  (Fig.  3.10).  This  is  done  to  reduce  the  size  of  the  vertex  list  sent 
to  the  edge  simplification  algorithm.  While  not  required,  the  vertex  simplification  is 
simple  and  quick,  and  speeds  up  the  overall  line  simplification  process. 

A  tolerance  value  r  is  defined  such  that  for  any  point  V),  if  A (V),  V)+m)  <  r  for 
m  =  {i,  i  +  1,  i  +  2,  i  +  3,  ...n},  then  Vl+m  is  removed  from  V.  When  A (V),  Vi+m)  >  r 
then  i  is  set  i  <—  i  +  m.  Refer  to  (Appendix  A,  Fig.  A. 4)  for  pseudo  code  of  this 
algorithm.  The  Douglas-Peucker  algorithm  [9]  uses  the  closeness  of  a  vertex  to  an 
edge  segment  as  judgment  criterion  for  elimination.  It  starts  with  a  crude  initial 
guess  at  a  simplified  polyline,  i.e.  the  single  edge  joining  the  first  and  last  vertices 
of  the  polyline.  The  remaining  vertices  are  tested  for  closeness  to  that  edge.  If 
these  vertices  are  further  than  a  specified  tolerance  from  the  edge,  then  the  furthest 
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ExtractPoints(V,  H) 

For  (0  to 

Hy  |  as  j  ) 

For  (0  to 

1  \HX\  as  i 

If 

>  e) 

Ps  <- 

Pe  <- 

Ps 

3  <- 

\Hy\ 

i  | 

Hx  | 

DO 

V<-VU{ps} 

Hv  p  < —  0 

PSX  iFSy 

ps  <—  Next  closest  occupied  cell 
If  (A (p3,pe)  >  7)) 

Mark  point  this  as  the  end  of  a  polyline 
Pe  <-  Ps 

UNTIL  (\H\  =  0) 


Figure  3.9  ExtractPoints 

vertex  from  it  is  added  to  the  simplification.  This  process  creates  a  new  guess  for 
the  simplified  polyline.  Applying  this  process  recursively  for  each  edge  until  all 
vertices  of  the  original  polyline  are  within  tolerance  of  the  simplification  yields  the 
final  simplified  set  of  edges. 

More  specifically,  two  endpoints  of  a  polyline  are  connected  with  a  straight  line 
as  the  initial  rough  approximation  of  the  polyline.  Computing  the  distances  from 


Figure  3.10  Vertex  simplification  process. 
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all  intermediate  polyline  vertices  to  that  (finite)  line  segment,  allows  us  to  judge 
how  well  it  approximates  the  whole  polyline.  If  all  these  distances  are  less  than  a 
specified  tolerance  rj ,  then  the  approximation  is  acceptable  and  the  endpoints  are 
retained  while  the  other  vertices  are  eliminated.  If  any  of  these  distances  exceed  rj, 
the  approximation  is  not  considered  good  enough  to  use.  In  this  case,  the  point  that 
is  furthest  away  is  selected  as  a  new  vertex  subdividing  the  original  polyline  into 
two  shorter  polylines  [9].  The  simplify  function  is  initialized  with  Wo  =  Wn  =true, 
where  Wo  is  the  first  vertex  and  Wn  is  the  last  vertex.  Let  Wo  be  the  set  of  output 
vertices.  The  full  algorithm  is  shown  in  (Appendix  A,  Fig.  A. 5). 


3.1.8  Line  Fitting.  The  final  step  of  the  process  involves  using  vertices 
returned  by  the  Douglas-Peucker  algorithm  as  end  points  to  fit  a  linear  regression 
line  [25]  of  the  form  y  =  b0  +  b±x  using  the  least  squares  method  where 


bi  = 


n  E"=i  XiVi  -  (EILi  Xi)  (EE  Vi) 


n  Vn  x2  — 

"  Z_a=i  -Li  \A^i= 1 


Xi 


(3.8) 


bo  =  y  -  hx 


(3.9) 


Letting  x  and  y  be  the  averages  of  the  x  and  y  points  respectively. 

Pearson’s  correlation  coefficient  [25]  p  is  calculated  to  determine  line  suitability 
for  a  linear  fit  and  if  not,  reject  the  fitted  line.  The  coefficient  p  is  represented  as: 


_ n  E  xv  -  E  x  E  y _ 

\j (n  E  ^’2  -  (E  x)2)  (n  E  y2  -  (E  y)2) 


(3.10) 


A  line  with  |p|  <  .9  (testing  showed  .9  as  a  value  which  produced  the  best  results) 
where  —  1  <  p  <  1,  is  classified  as  a  bad  correlation  in  our  implementation.  The 
Douglas-Peucker  algorithm  restricts  this  from  allowing  small  noise  data  to  produce 
large  errors  in  the  output  map  (Fig.  3.11)  requiring  this  extra  fitting  process.  This 
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process  will  work  equally  well  if  the  mapped  wall  is  straight  or  curved.  The  Douglas- 
Peucker  algorithm  produces  more  vertices  when  simplifying  a  rounded  environment 
then  it  does  in  a  symmetric  environment.  The  increased  vertices  allow  the  map  to 
maintain  its  curved  geometric  data. 


#e  ©eQ  q6  °©eee  Q 

(b) 

Figure  3.11  Line  fitting  before(a)  and  after(b)  application  of  a  linear  regression  fit. 

The  points  obtained  from  the  Douglas-Peucker  algorithm  are  used  as 
end  points  in  the  fitting  process. 


3.1.9  Localization  Algorithm.  Localization  is  a  computationally  expensive 
task,  especially  if  localization  is  performed  using  the  full  map  and  pose  dataset.  To 
overcome  this  time/complexity  issue,  the  robot  performs  localization  on  the  simpli¬ 
fied  cognitive  map  which  in  turn  corrects  map  errors.  Localization  is  accomplished 
using  the  Expectation  Maximization  (EM)  algorithm  finding  drift  error  for  each  ASR 
and  performed  iteratively  while  the  robot  maps. 

3. 1.9.1  Expectation  Maximization.  The  Expectation  Maximization 
(EM)  algorithm  [8]  is  used  often  in  computer  science  to  iteratively  calculate  the 
maximum  likelihood  of  some  distribution  with  incomplete  or  missing  data. 

Dempster  et  al.,  [8]  postulates  a  family  of  sampling  densities  as: 

9(x\4>)  =  [  f(q\(f>)dq  (3.11) 

Jq(x) 

The  EM  algorithm  seeks  to  find  parameters  0  that  maximizes  6(x |0)  with  observation 
x.  This  is  done  in  two  steps,  an  Expectation  step  (E-step)  and  Maximization-step 
(M-Step),  iteratively  converging  to  the  maximize  likelihood  of  /(g|0). 
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In  section  3. 1.9.2,  we  develop  a  model  of  the  Expectation  Maximization  algo¬ 
rithm  as  a  mixture  model  of  the  sonar  and  dead- reckoning  sensors  using  [8]  [24], 

3. 1.9. 2  Statistical  Model  of  the  EM  Algorithm  for  Localization.  We 
have  a  set  of  parameters  9k  =  (7^,  fa)  that  defines  a  probability  distribution  function 
of  the  robot’s  location,  where  k  =  s,  m;  s  being  the  abstracted  sonar  data  detailed 
in  Section  3.1.10  and  m  the  motor  data  of  the  probability  distribution  function. 

E-step:  During  the  E-step,  we  develop  the  expected  complete  log-likelihood 
function,  Q(9,9',  (xi,x2,x3,  ...,xjv))  based  on  the  previous  guess  9'  and  the  data  set 
(xi,  x2,  x3 , ...,  xn)  which  is  the  combined  data  {range  U  motor}.  This  step  produces 
a  new  better  guess  of  the  robot’s  pose  ffan  and  sensor  data  fis  and  is  also  used  for 
the  calculation  of  of2  and  fa. 

Let  Q  be  the  expected  value  of  the  log-likelihood  given  the  data  (x3, ...,  x^) 
and  (pk 

'  N 

Q{9,9',  (xi,  •••,  xN))  =  E  EE  log  (Pr(gf ,  Xi ;  9))  \x  =  (xi, ...,  xN) 

_i=l  k 

N 

=  log  (Pr(Afc’  xl  °))  Pr(^kd  °') 

i= 1  k  q 

N 

=  EEE  Qi  log(vrfc  Pr(xi\fa))  Pr(^|xi5  9') 

i=  1  k  q 

N 

=  EE  <  Qi  >  kp{xi\fa))  (3.12) 

i= 1  k 
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where, 


<9?>=E«."Pr(«."  i^9') 


Pr(aq;  O') 


n’pfa  Wk) 

EfcPr(^l^;0/)Pr(^) 


<PN</4) 

£*<p(s*l$fe) 


(3.13) 


Substituting  our  calculation  for  the  M-Step  (Eq.  3.17)  into  (Eq.  3.13)  we  get 


exp  - 


TTfc' 


(xj-tLyY 


k' 


<  <£  >= 


V& 0< 


°v 


exp  - 


7To 


\J  (2n)a 


4-  7T/ 

i  ' 1  rn 


exp  - 


\/  (27r)° 


(3.14) 


Substituting  (Eq.  3.14  and  3.16)  into  (Eq.  3.12)  we  get 


Q(M',  (aq,  ...,xN))  = 


N 

E 

i=l 


(*<-*¥) 


\J  (27r)a 


log  7T; 


.  (xa—^sY 

expl  _  2^T“ 


\/  (2tt)(7 


exp  - 


vr; 


+  7T/ 


exp 


\A^0ct 


714 


,  {xi-»rr,iY 

expl - 5^*“- 


\/(27 r)o 


log  7T, 


-pi 


V& 


exp  - 


7Tl- 


(**-#v)2 


exp  - 


\/  (27r)CT 


+  7T/ 

l  ' 1  rn 


(xa  —  u,  /)2 
v  *  ' 


(3.15) 
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M-step:  Find  the  value  of  6  that  maximizes  Q.  Assuming  that  sensor  noise 
and  motor  error  fit  a  Guassian  distribution,  it  follows: 


Pr(x| /i,  a2)  = 


exp 


G-m)5 

2ct2 


a/(2t r)(T 


(3.16) 


{x-fls)2\  f  {X-Hmj2 


exp 


2cr2 


exp 


+ 


2o-2 


v^F)^ 


(3.17) 


Add  the  Lagrange  multiplier  A  to  (Eq.  3.12),  we  optimize, 


N 


0pt\  <  qi  >  l0g  (7r*P  +  A  1  - 


,2=1 


(3.18) 


and  take  the  partial  derivatives  with  respect  to  7ik  and  (pk  =  (//*,  crk)  to  get, 


dQ_ 
di rfc 


N 

E 

2=1 


<  gj  > 


Eii  <  <hfc  > 


A 


TTfc  ~ 


AT 


(3.19) 


<9Q 

<9/ife 


<  gf  >  (^»  ~  /ffc)  __  g 

2—--  rr? 


2=1 


N  k  N  u 

ST'  <  Qj  >  _  \  ^  <  Qj  >  hfc 

“  o\  ^  al 

2=1  ^  2=1  K 


=  0 


N  i,  N  u 

V  ^  <  gf  >  hfc  _  \  ^  <  Qj  >  Xj 

2=1  ^  2=1  ^ 
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TV 


TV 


U  u  U  h. 

K  2=1  K  2=1 


N 


N 


l-ik  ^2<Qi  >=^2  <  Qi  >  xi 


1=1 


i=  1 


l^k  — 


Ely  k- 

i= i  <  <K  >  m 

Eii  < 9?  > 


(3.20) 


dQ_ 

dai , 


TV 

E 

2=1 


<  9*  >  [0*  -  /ifc)2  -  crl 


=  0 


err 


Z^ 

2=1 


<  Qi  >  {Xi  ~  Hk)2  v-  <  $  >  =  o 


“  crl 

i=i  K 


Z^ 

2=1 


<  gf  >  (gj  -  z fc )2  =  ^  <  gf  > 


07 


2=1 


Cfc 


TV  TV 

— <  g*  >=  nr  <  9?  >  (®*  -  /^-)2 


CTk 


2=1 


0" 


3  ^ 

^  2=1 


^  Eli  <  9?  >  (*<  ~  /*fc)2 

^  Ell  <  9?  > 


~2  Eli<9?> 

CTk  ~  ~ 


Xi 


% kY 


Eli  <  9?  > 


(3.21) 
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Substituting  (Eq.  3.14)  into  (Equations  3.19,  3.20  and  3.21)  we  get, 


7T k 


N 
l^i= 1 


\ 

/ 


AT 


(3.22) 


<?k 


(3.23) 


(3.24) 


The  E  and  M  steps  of  the  EM  algorithm  are  repeated  until  convergence. 

1.  E-Step:  Compute  Q{9,9',  (®i,  -,xN))  =  Eii  J2k  <  9*  >  log(7rfcp(xj|0fe)) 

2.  M-Step:  Find  {i Tfc,/^,  <Jfc2} 

3.  Repeat  until  convergence. 

The  pseudo  code  for  the  EM  algorithm  can  be  found  in  Appendix  A,  Section  A. 3. 
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3.1.10  Line  Direction.  To  calculate  s  from  each  ASR,  a  direction  for  each 
line  within  an  ASR  is  calculated  to  obtain  an  ASR  direction  (Fig.  3.12).  Each  ASR 


Figure  3.12  ASR  directions  needed  to  perform  localization  correction. 

contains  a  list  of  vertices  representing  the  polylines  that  make  up  the  ASR.  Every 
two  vertices  is  a  line  segment  and  the  direction  of  each  line  segment  is  obtained 
by  treating  it  as  a  vector  and  calculating  the  angle  of  the  vector.  Slight  variations 
in  these  angles  (Fig.  3.13),  caused  by  noise  or  obstacles,  results  in  error  that  is 
accounted  for  using  the  EM  algorithm,  as  described  in  the  previous  section.  Let 


Figure  3.13  Vectors  shown  for  a  single  ASR. 

A  denote  the  current  vector  and  B  the  vector  that  comes  before  A.  The  origin 
of  A  is  the  termination  of  B  and  normalization  of  A  is  accomplished  by  setting 
Ax  <—  Ax  —  Bx  and  Ay  <—  Ay  —  By.  To  find  the  angle  of  A  let  U  be  the  normal  vector 
along  the  positive  x  axis  of  the  cartesian  map  represented  by  the  current  ASR,  use: 


(3.25) 
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Because  U  is  the  unit  vector  along  the  x  axis,  (Equation  3.25)  simplifies  to: 


=  cos 


-i 


Ax  x  1  +  Ay  x  0 


l-4|  x  1 


(3.26) 


To  get  a  general  direction  for  an  ASR  we  need  to  normalize  all  vector  angles 
within  the  ASRs  so  that  we  only  consider  a  vector  to  be  pointing  in  quadrants  I  (0 
to  90  degree)  and  IV  (0  to  -90  degrees).  Additionally,  each  angle  is  only  considered 
as  it’s  offset  from  the  positive  x  axis.  Because  of  the  generally  symmetric  and 
perpendicular  nature  of  indoor  environments  most  vectors  are  parallel  or  normal  to 
each  other;  so  rather  than  creating  a  set  containing  two  distributions  (one  at  around 
ninety  degrees  and  another  around  zero  degree)  any  angle  that  is  greater  then  forty 
five  degrees  is  decreased  by  ninety  degrees  to  acquire  its  normal  angle.  These  steps 
are  detailed  in  the  following  text. 

Initially  0  <  0  <  180  but  this  does  not  tell  us  if  the  angle  is  negative  or 
positive.  To  determine  negative  angles,  if  Ax  <  0  use: 


0  < - 0.  (3.27) 

Now  to  adjust  each  line  so  that  they  all  point  to  the  right,  if  0^  >  90°  (Fig.  3.14a) 
then  the  recorded  (p'A  is  (Fig.  3.14b): 


0(4  =  180°  -  0A 


(3.28) 


and  if  4>a  <  —90°  then: 


0(4  —  180°  +  <f>A 

if  neither  of  these  conditions  are  met  then  A'  =  A. 


(3.29) 
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90 


90° 


(b)  Angle  adjustment  when  the 
angle  is  greater  than  90° 


90 


(c)  Angle  adjust  if  the  new  angle  is  greater  than  45° 

Figure  3.14  Angle  directions  are  adjusted  to  point  as  close  as  it  can  along  the 
positive  x  axis. 


Finally,  if  (j)'A  >  45°  then  (Fig.  3.14c): 


A  =  90c 


(3.30) 


and  if  <p'A  <  —45°  then: 


A'  =  -90°  - 


(3.31) 


3-23 


It  is  important  to  understand  that  this  only  gives  us  the  s  data  set.  This  data, 
along  with  the  center  point  of  the  exit  locations,  the  m  data  set,  is  sent  to  the  EM 
algorithm  as  described  in  Section  3. 1.9.1  to  find  the  actual  direction  of  the  ASR. 
The  EM  algorithm  then  uses  the  vectors  and  their  positions  by  bringing  (f)'A  to  the 
same  orientation  from  the  x— axis  and  moving  the  origins  of  A  to  its  most  probable 
location.  The  difference  between  the  current  ASR  and  the  previous  one  is  used  as 
the  error  in  localization  to  correct  the  robots  pose.  The  algorithm  to  fill  the  s  data 
set  is  shown  in  (Appendix  A,  Fig.  A. 7). 
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IV.  Results  and  Analysis 

Testing  the  implementation  of  the  mapping  system  is  accomplished  modularly  for 
each  algorithm,  and  as  a  complete  system.  Results  of  these  tests  are  shown  and 
explained  in  the  remainder  of  this  chapter. 

The  first  series  of  tests  includes  running  the  system  using  the  Pioneer  robot 
simulator  which  simulates  the  Pioneer  P2-AT  robot.  The  Pioneer  P2-AT  robot 
possesses  a  400Mhz  Pentium  processor  and  64MB  of  random  access  memory.  Cell 
size  in  all  results  is  10cm  x  10cm  by  setting  <p  =  100.  Additionally,  thresholds  are  set 
as  shown  in  (Table  4.1). 


Table  4.1  T 

iresholds  and  parameters  used  while  mapping. 

Symbol 

Value 

Description 

e 

7 

The  value  in  a  cell  deemed  occupied 

T 

2 

Closest  allowed  distance  of  a  vertex 

7 

4 

Threshold  for  the  Douglas-Peucker  algorithm 
to  determine  edge  angle  cutoff 

P 

100 

Scalar  used  to  determine  resolution  of  the  map 

Each  of  these  thresholds  were  extensively  tested,  and  these  were  found  to 
produce  the  clearest  map  under  the  largest  variety  of  mapping  circumstances  (large, 
small,  cluttered,  and  uncluttered  environments).  Increasing  r,  reduces  the  size  of 
the  final  polyline  map  but  will  decrease  the  detail  of  the  map  at  the  same  time. 
Changing  the  size  of  the  scalar  <p  results  in  a  inversely  proportional  change  in  the 
memory  size  of  the  map.  Additionally,  mapping  was  accomplished  in  real  time  with 
the  robot  traveling  at  approximately  0.5m/s. 

The  map  in  (Fig.  4.1a)  shows  a  map  created  using  the  Pioneer  robot  simulator 
that  includes  9  ASRs,  and  those  rooms  represented  as  a  graph  (Fig.  4.1b).  In 
(Fig.  4.2)  an  example  ASR  before  and  after  conversion  to  its  simplified  polygonal 
representation,  mapped  from  rooms  5  and  6  of  (Fig.  4.1a).  The  polygon  requires 
less  than  0.1KB,  while  the  MHIMM  used  42KB  for  representing  the  rooms.  In  (Fig. 


4-1 


4.4)  nine  ASR’s  are  mapped.  When  the  rooms  are  put  back  together  after  mapping 
is  complete,  localization  errors  have  caused  overlapping;  (Fig.  4.5)  shows  a  similar 
map  with  less  localization  error. 

In  (Table  4.2)  the  respective  hie  sizes  of  each  representation  of  the  map  in  (Fig. 
4.6)  is  shown.  Each  stage  of  the  process  provides  a  substantially  reduced  data  set, 
and  (Fig.  4.6d)  is  only  1.06%  the  size  of  (Fig.  4.6b). 


Table  4.2  Size(KB)  differences  of  map  representations 


Map 

Size 

(Fig.  4.6a) 

784 

(Fig.  4.6b) 

56 

(Fig.  4.6d)  and  (Fig.  4.6f) 

8 

In  (Fig.  4.1b)  the  separate  ASRs  are  shown  as  a  graph  with  their  corresponding 
representations  to  the  map  in  (Fig.  4.1a).  This  map  was  generated  using  the  Pioneer 
robot  simulator. 


Figure  4.1  ASR’s  Represented  as  a  Graph.  The  actual  map  is  shown  in  (a).  The 
graph  (b)  shows  how  the  rooms  that  were  mapped  translates  to  a  graph. 


Enlarging  part  of  the  map  corresponding  to  rooms  five  and  six  of  (Fig.  4.1a)  in 
(Fig.  4.2a)  is  the  array  map  with  the  rooms  reconstructed  to  their  proper  location. 
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The  final  polyline  map  (without  localization)  of  these  two  rooms  is  shown  in  (Fig. 
4.2b). 


Figure  4.2  A  2  Room  ASR  Example  showing  the  polygonal  representation  (b)  of 
the  original  HIMM  map  (a).  This  figure  is  rooms  5  and  6  of  (Fig.  4.1a). 

In  (Fig.  4.3)  the  complete  process  of  mapping  the  nine  rooms  is  shown  without 
the  localization  step.  As  the  robot  moves  from  room  to  room  the  array  map  is  created 
(left  side  of  (Fig.  4.3))  and  cleaned  (right  side  of  (Fig.  4.3)).  But  only  the  polyline 
information  is  maintained  as  the  robot  moves  onto  the  next  room.  This  process  is 
accomplished  real  time  while  the  robot  maps  the  environment,  without  localizing. 


4-3 


Cfe  Oj 


Figure  4.3  ASR  Development 
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In  (Fig.  4.4)  is  the  completely  reconstructed  map  from  (Fig.  4.3),  also  showing 
the  path  of  the  robot. 


Figure  4.4  Nine  ASR’s  Merged  Together. 


Applying  the  full  SLAM  algorithm,  mapping  only  rooms  five,  six,  and  seven 
of  (Fig.  4.4)  gives  the  map  in  (Fig.  4.5b).  The  unlocalized  map  is  shown  in  (Fig. 
4.5a)  for  comparison.  Using  the  system  in  a  real  world  area  produced  better  results 
then  when  using  the  Pioneer  robot  simulator.  We  show  in  (Fig.  4.6)  a  mapping  of 
the  first  floor  of  the  engineering  building  at  the  Air  Force  Institute  of  Technology, 
the  black  lined  area  of  (Fig.  3.1),  minus  some  offices  with  dimensions  approximately 
(255'  x  255').  The  robot  is  this  figure  is  sent  around  the  hallway  one  time  with  no 
overlap  of  areas  mapped.  On  the  left  (Fig.  4.6a),  is  the  grid  representation  of  the 
map.  On  the  right  (Fig.  4.6b),  the  vertex  map  without  simplification  is  illustrated 
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Figure  4.5  Three  ASRs  merged  together  showing  little  localization  error  to  correct, 
(a)  is  the  unlocalized  polyline  map  and  (b)  is  the  localized  polymap. 

with  a  piece  of  the  map  expanded  to  show  greater  detail  in  (Fig.  4.6c).  In  (Fig. 
4.6d)  is  the  final  simplified  polyline  map  without  localization  with  the  top  right 
corner  blown  up  in  (Fig.  4.6e).  After  localization  (Fig.  4.6f)  the  map  is  almost 
corrected  however  some  areas  remain  within  the  map  that  were  unable  to  localize 
leaving  slight  imperfection.  However,  if  there  were  overlap,  i.e.  the  robot  revisits  the 
location,  this  localization  error  most  likely  would  be  rectified. 


4-6 


Figure  4.6  The  polyline  map  of  the  first  floor  of  the  engineering  building  at  the  Air 
Force  Institute  of  Technology.  On  the  left  (a),  is  the  grid  representation 
of  the  map,  (b)  is  the  vertex  map  without  simplification,  (c)  shows  a 
blowup  of  part  of  the  map.  In  (d)  is  the  final  simplified  polyline  map 
without  localization,  (e)  is  a  blowup  of  part  of  (d).  The  map  after 
localization  is  shown  in  (f). 
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4-1  Small  Complex  Mapping  Regions 


Testing  small  complex  spaces  at  first  posed  something  of  a  problem  which  is 
solved  by  decreasing  the  scalar  p.  The  scaling  factor  used  in  the  larger  maps  resulted 
in  incoherent  smaller  maps  (Fig.  4.7).  This  map  was  made  using  the  Pioneer  robot 
simulator.  A  real  world  environment  is  shown  later  producing  similar  results. 


Figure  4.7  Stages  of  an  example  using  the  Pioneer  robot  simulator  of  a  very  small 
area  being  mapped.  Area  is  approximately  20 ft  x  40 ft.  The  scalar  p  is 
set  equal  to  85. 
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However,  reducing  the  scalar  p  from  100  to  55  for  more  complex  regions  pro¬ 
duces  a  better  map  (Fig.  4.8). 


Figure  4.8  Stages  of  an  example  using  the  Pioneer  robot  simulator  of  a  very  small 
area  being  mapped.  Area  is  approximately  20/*  x  40/*.  The  scalar  p  is 
set  equal  to  55. 


If  the  region  to  be  mapped  is  a  large  simple  region,  the  reduced  scaling  factor 
makes  little  difference  except  perhaps  an  increased  processing  time  as  more  infor¬ 
mation  must  be  processed.  Because  of  the  small  memory  space  needed  using  this 
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mapping  system,  the  scale  can  be  reduced  very  low  even  in  a  large,  highly  complex 
area  with  the  potential  to  provide  a  surprisingly  detailed  map. 

To  test  the  small/messy  type  of  environment  in  a  real  world  environment, 
the  robot  was  sent  through  part  of  another  building  (the  AFIT  robotics  lab)  which 
currently  has  items  and  furniture  laying  about  almost  randomly  (Fig.  4.9).  In  this 
figure  all  the  ASRs  are  shown  as  the  raw  array  maps  along  with  the  robot  path. 


Figure  4.9  Small  messy  real  world  environment 


(a) 


(b) 


Figure  4.10  Real  world  polyline  map  of  a  small  cluttered  region  before  (a)  and 
after  (b)  localization. 
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4-2  Localization 

Applying  the  localization  algorithm  in  the  hallway  of  the  AFIT  building,  which 
is  a  relively  simple  environment  but  very  large,  produces  the  map  in  (Fig.  4.11).  This 
environment  represents  the  ideal  arena  for  the  SLAM  algorithm  implemented  in  this 
thesis. 


Figure  4.11  Localized  map  of  the  AFIT  hallway. 
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Figure  4.11  is  shown  overlaid  with  the  blueprint  in  (Fig.  3.1)  in  (Fig.  4.12). 


Figure  4.12 


Polyline  map  of  the  AFIT  hallway  overlaid  with  the  blueprint. 
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The  processing  requirements  to  perform  the  mapping  in  (Fig.  4.11)  does  not 
incur  any  constraints,  because  the  data  set  in  which  localization  was  performed  on 
is  so  small.  The  total  processing  time  in  a  batch  mode  with  all  of  the  data  from 
a  recorded  robot  took  38  seconds.  A  graph  showing  the  processing  time  taken  to 
process  1  to  43  of  the  4m  fixed  sized  ASRs  in  (Fig.  4.11)  is  shown  in  (Fig.  4.13). 
The  processor  time  is  calculated  without  included  time  required  to  traverse  the 
environment  and  acquire  sensor  readings. 


Figure  4.13  Processor  time  comparison  required  at  which  ASR  in  (Fig.  4.11) 

The  complete  SLAM  process  performs  in  real-time.  The  robot  is  sent  through 
the  hall  and  as  the  robot  progresses  through  the  environment,  ASRs  are  constructed 
and  compared  against  the  previous  ASR  to  perform  localization. 
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V.  Future  Work  and  Conclusion 


While  many  methods  exist  for  robots  to  map  their  environment  [4]  [20]  [26]  [17]  [34], 
as  the  size  of  these  environments  increases,  methods  using  occupancy  grids  start 
to  approach  maximum  constraints  [4]  [26]  [34],  The  two- fold  method  (polylines  and 
ASRs  using  sonars)  [4]  [11]  [17]  combines  the  best  of  two  robot  mapping  philosophies. 
Our  system  makes  it  possible  for  a  robot  armed  only  with  sonars  to  map  an  arbitrarily 
large  area.  It  is  size  scalable,  with  a  non-increasing  computational  time  required  to 
map  and  a  linearly  increasing  computational  time  required  to  reconstruct  the  ASRs 
after  completion  of  mapping,  that  is,  create  a  single  global  map  using  the  individual 
ASRs.  Additionally,  the  reduced  data  set  that  makes  up  the  polylines  are  used  to 
perform  localization  using  the  Expectation  Maximization  (EM)  algorithm. 

The  objectives  of  this  thesis  was  to: 

1.  Implement  and  test  a  fast  histogram  mapping  algorithm  which  was  done  using 
a  modification  to  the  HIMM  mapping  technique. 

2.  Develop  a  polygonal  and  topological  representation  from  the  occupancy  grid 
map.  This  was  accomplished  using  a  new  technique  of  filtering  the  HIMM 
map  and  converting  the  data  to  a  vertex  list  and  applying  the  Douglas  Peuker 
algorithm  to  simplify  the  list. 

3.  Implement  and  test  an  Expectation  Maximization  algorithm  to  perform  local¬ 
ization  corrections  of  the  cognitive  map.  This  was  also  successfully  accom¬ 
plished  fitting  polygonal  data  into  a  representation  which  could  be  maximized 
with  the  EM  algorithm. 

All  objectives  were  successfully  completed  as  was  the  goal  for  providing  a  fast 
(using  HIMM  technique),  accurate  (EM  and  linear  fitting)  method  using  probability 
theory,  to  quickly  map  a  large  area.  Additionally,  the  map  is  stored  in  a  small  amount 
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of  memory  (polygonal  ASR  representation)  where  if  needed,  it  conld  be  transmitted 
quickly  to  another  device. 

The  cognitive  mapping  process  allows  robots  to  easily  implement  corrections 
to  localization  errors  using  the  simplified  ASRs  instead  of  grid  data.  In  figure  5.1,  a 
before  and  after  map  is  shown  with  a  memory  footprint  of  the  map.  Additionally, 
the  ASR  is  reduced  to  a  fixed  size  of  4m,  allowing  the  robot  to  perform  iterative 
localization  using  an  expectation  maximization  algorithm  similar  to  Thrun,  et  al. 
[36],  matching  the  current  array  window  of  the  robot  with  the  previous  ASRs  to 
determine  the  more  probable  current  pose.  This  idea  is  taken  from  work  developed 
by  Schultz  and  William  [33],  where  range  sensor  data  is  divided  up  into  several  time 
slices  and  comparing  them  to  determine  an  estimate  of  pose  error.  Our  idea  is  to 
extend  this  technique  to  ASRs  and  fix  the  size  of  the  ASR  to  some  small  value  then 
compare  them  against  each  other  in  a  similar  fashion.  This  technique  produces  good 
results.  The  map  in  Figure  5.1b  is  the  polygonal  representation  of  the  map  in  Figure 
5.1a.  After  localization  adjusting  rotation  sensor  error  among  room  ASRs,  yields 
Figure  5.1c  (this  is  the  same  map  shown  in  (Fig.  4.11)). 


Figure  5.1  The  array  map  of  the  first  floor  of  the  engineering  building  at  the  Air 
Force  Institute  of  Technology  is  shown  in  Figure  (a).  Figure  (b)  shows 
the  polyline  map  with  localization  errors  uncorrected,  figure  (c)  shows 
the  polyline  map  after  rotational  localization  errors  are  fixed. 
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5. 0. 1  Extensions.  The  technique  used  in  this  thesis  affords  the  opportunity 
of  several  extensions.  While  sonars  are  very  common  in  robot  mapping  systems,  they 
are  an  older  active  method  of  sensing.  The  term  active  describes  a  sensor  that  needs 
to  send  some  sort  of  signal  to  take  a  measurement,  unlike  a  passive  sensor  which 
requires  no  emitted  signal  to  gain  a  measurement.  Replacing  or  enhancing  range 
data  with  a  passive  camera  system  could  produce  a  cleaner,  more  accurate  map 
while  at  the  same  time  not  generate  any  detectable  signal. 

Using  ASRs  as  a  means  of  representing  the  map  lends  itself  to  multi-robot 
mapping.  It  is  an  easy  extension  to  use  multiple  robots,  each  mapping  separate 
ASRs  at  the  same  time  and  at  some  point  communicating  the  maps  to  each  other 
and  merging  them  to  one  comprehensive  map.  This  would  require  some  additional 
calculations  to  determine  where  each  robot  is  with  respect  to  each  other  but  not 
much  more. 

This  implementation  places  a  low  emphasis  in  exit  detection.  Many  possible 
configurations  of  exits  exist  including: 

1.  A  room  into  another  room  through  a  doorway  (Fig.  5.2a) 

2.  Room  to  a  hallway  that  leads  to  yet  another  room  (Fig.  5.2b) 

3.  From  the  middle  of  a  room  to  a  hallway  to  the  side  of  another  room  (Fig.  5.2c) 

4.  Side  of  a  room  to  a  hallway  to  the  side  of  another  room  (Fig.  5. 2d) 

5.  Same  but  no  hallway  (Fig.  5.2e) 

6.  Partition  in  one  room  (Fig.  5.2f) 

These  are  just  a  few  of  the  many  possible  configurations.  Of  this  list,  only  (Fig. 
5.2a),  (Fig.  5.2e),  and  (Fig.  5.2f)  can  be  detected  with  the  algorithm  used  in  this 
thesis.  An  obvious  extension  is  to  include  a  more  sophisticated  exit  detection  system 
that  can  handle  a  wider  variety  of  exit  configurations. 
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(b) 


(d)  (e)  (f) 


Figure  5.2  Various  types  of  exits  the  robot  may  typically  encounter. 

While  this  system  will  map  an  environment  that  changes  slightly  with  time 
without  a  serious  degradation  of  the  map  it  is  not  designed  specifically  to  handle  dy¬ 
namic  environments.  Another  enhancement  to  the  system  is  the  addition  of  an  agent 
responsible  for  identifying  changes  to  the  environment  and  sequentially  registering 
those  changes  to  the  appropriate  ASR. 
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Appendix  A.  Algorithms 


A.l  Mapping  Algorithms 

Section  2.2  and  Section  3.1.3  gives  the  definitions  shown  in  (Table  A.l)  that 
are  used  in  this  section  to  develop  a  general  algorithm  for  grid  mapping. 


Table  A.l  Symbol  key  for  the  mapping  algorithms. 


Symbol 

Definition 

P 

Scalar  used  to  determine  cell  size. 

C 

2  dimensional  matrix  that  represents  the  world. 

m 

Max  distance  divided  by  p  expected  in  the  x  direction. 

n 

Max  distance  expected  to  travel  in  the  y  direction  divided  by  p. 

h 

Robots  direction. 

shi 

Each  sonar’s  angle  relative  to  the  robot. 
i  =  {1,  2,  3,  ...number  of  sonars} 

K 

The  global  angle  of  each  sonar,  hi  =  h  +  shi. 

SOXi 

Sonar *n  x  offset  from  the  center  of  the  robot. 

soyi 

Sonarpi  y  offset  from  the  center  of  the  robot. 

Map(C) 

Loop 

so  < —  New  sonar  readings 
h  GetPoseH (robot)  / /Vehicle  direction 

xq  <—  GetPoseX (robot)  / /Vehicle  center  point  x  coordinate 
y0  <—  GetPoseY (robot)  /  /Vehicle  center  point  y  coordinate 
For  (i  =  1  to  number  of  sonars) 

riXcos(hi)-\-(soXi  cos (h)—soyi  sin (h)) 

1  p 

riXsin(hi)-\-(soXi  sin (h)—soyi  cos (h)) 

Vi  <  P 

Update(C'J;i)2/i) 

End  For 
End  Loop 

Figure  A.l  General  Occupancy  Grid  Mapping  Algorithm 
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Map(C) 

Loop 

so  <—  New  sonar  readings 
h  <—  GetPoseH (robot)  //Vehicle  direction 
xq  GetPoseX (robot)  //Vehicle  center  point  x  coordinate 
Do  <—  GetPoseY  (robot)  //Vehicle  center  point  y  coordinate 
For  (i  =  1  to  number  of  sonars) 

ri  xcos(/ii) +(soxi  cos (h)—soyi  sin (h)) 

Xi  p 

riXsin(hi)+(soXi  sin (h)—soyi  cos (h)) 

Vi  p 

If  (dist(xi,yi,x0,yo)  <  w)  Than  Update^^J 
End  For 
End  Loop 

Update(C'tXjtj/) 

Ctx,ty  =  Gtx,ty  +  1-5 

For  (l  =  —1  to  1) 

For  (m  =  —1  to  1) 

Ctx+m,ty+l  —  Ctx+m,ty+l  “I-  1-5 

If  (Ctx+m  ,t.y+i  P  15)  than  (Ctx+mjy+i  15) 

End  For 
End  For 

//decrease  all  cells  by  1  in  the  line  from  ( tx,ty )  to  (xo,yo)-  ^ an  ^ roP  ^ 0 
Bresenham’s  line  algorithm  [6]  is  used  to  follow  the  line. 

LineDecrease(fx,  ty,  xq,  yo) 

Figure  A. 2  Histogram  In  Motion  Mapping  Algorithm 
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Map(C) 

Loop 

so  <—  New  sonar  readings 

h  <—  GetPoseH (robot)  //Vehicle  direction 

xq  <—  GetPoseX  (robot)  //Vehicle  center  point  x  coordinate 

2/o  GetPoseY  (robot)  //Vehicle  center  point  y  coordinate 

For  (i  =  1  to  number  of  sonars) 

ri  xcos(hi) +(soXi  cos (h)—soyi  sin (h)) 

®  p 

r{  x  sin  (hi) -j-(s  ox  i  sin  (h)—soyi  cos(h)) 

If  (dist(xi,yi,x0,yo)  <  w)  Than  Update(C,  Xi,  yu  x0,  y0) 

Else 

wx cos(hi)-\- (soxi  cos (h)—soyi  sin (h)) 
xi  p 

wxsin(hi)-j-(soxi  sin (h)—soyi  cos (h)) 

Vi  *  P 

//decrease  all  cells  by  1  in  the  line  from  (xi,xf)  to  (xo,yo).  Can  drop  to  -3. 
LineDecrease((7,  U,ti,xo,yo) 

End  For 
End  Loop 


Update(C,  tx,  ty,  x0,y0) 

Ct0,t0  <-  -100 

Gtx,ty  i  Ctx,ty  "F  1-5 

For  (l  =  —  1  to  1) 

For  (m  =  —1  to  1) 

Ctx+m,ty+l  <  "F  1-5 

If  (Ctx+m.ty+l  P  15)  than  Ctx+m,ty+l  i  15 

End  For 
End  For 

LineDecrease((7,  tx,  ty,  xq  ,  yo) 

//decrease  all  cells  by  1  in  the  line  from  (tx,ty)  to  (xo,yo)- 
//Allowed  to  drop  to  -3. 

//Bresenham’s  line  algorithm  [6]  is  used  to  follow  the  line. 


Figure  A. 3  Modified  Histogram  In  Motion  Mapping  Algorithm 


A. 2  Simplification  Algorithms 


The  process  of  simplifying  the  list  of  vertices  V  extracted  from  the  map  begins 
by  eliminating  clusters  of  closely  grouped  points.  A  tolerance  value  r  is  defined  such 
that  for  any  point  If,  if  A  (V*,V*+m)  <  r  for  m  =  {i,  i  +  l,i  +  2,  i  +  3,  ...n},  then 
V,:+m  is  removed  from  V.  When  A  (Vi,  Vi+m)  >  r  then  i  is  set  i  <—  i  +  m. 


VertexSimplify(F) 

e  =2  /*The  bigger  e  the  less  accurate  the  map  will  be  2  seems  to  work 
good.  */ 

For  (from  0  to  \V\  —  2  as  i) 

For  (from  i  to  \V\  as  m ) 

A(Vr,Vr+m). 

If  (d  <  e)  V  =  V  —  {Vi+m}  /*that  is,  Vi+m  is  removed  from  V  */ 

If  d  >  e  Then  Break 


Figure  A. 4  VertexSimplify  Algorithm 


Let  r]  be  a  specified  tolerance  for  the  smallest  angel  algorithm  should  recurse 
on  where  if  the  angle  is  greater  than  77,  then  the  approximation  is  acceptable  and  the 
endpoints  are  retained.  The  simplify  function  is  initialized  with  Wo  =  Wn  =true, 
where  Wo  is  the  first  vertex  and  Wn  is  the  last  vertex.  Let  Wo  be  the  set  of  output 
vertices. 
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PolySimplify() 

Simplify(?7,  W,  0,  n,  mark ) 

For  (0  to  \W\  as  i) 

If  (mar hi) 

then  add  it  to  the  output  set  Wo 
^Vonext  * 

Simplify(Float  g,  Set  L,  Int  j,  Int  k ,  Set  mark ) 

L  is  tfie  original  set  of  points  mark  is  the  vector  used  to  identify  points  to 
keep,  j  and  k  are  used  to  keep  track  of  the  current  and  last  point. 

If  ( k  <  j )  /* Nothing  to  simplify*/ 

Return 

Segment  S 

Sjk  Segment  from  Lj  to  Lk 

max  dist,ance  <  0 

maxi  <—  j 

Let  u  be  the  segment  direction  vector 
ux  <-  Sbx  -  Sax 
uy  <-  Sby  -  Say 
cu  *  (u  •  u) 

Pb  is  the  base  of  perpendicular  from  vl  to  S 
b,cw,dv2  /*dv 2  distfvi  ,  S2)*/ 

For  (j  +  1  to  k  as  i) 

dv 2  distance  squared  (Lj,  Pb) 

If  (dv 2  <  max/)  continue 

Else 

maxi  i 
rnaxd  dv2 

If  ( maxd  >  v) 

/* Split  the  polyline  at  the  farthest  vertex  from  S  */ 
markmaXi  <—  true 
simplify(r/,  L,  j,  max j,  mark ) 
simplify(r/,  L,  maxi,  k,  mark ) 

For(l  to  k  —  1  as  i) 

If((L(j_i)a.  <  0)  U  (Lix  <  0)  U  (L(i+i)it  <  0))  marki  trwe 
Figure  A. 5  Douglas-Peucker  Edge  Simplification  Algorithm 
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A. 3  The  EM  Algorithm 


We  have  a  set  of  parameters  6k  =  (7T*,,  fa)  that  defines  a  probability  distribution 
function  of  the  robot’s  location,  where  k  =  s,  m;  s  being  the  abstracted  sonar  data 
detailed  in  Section  3.1.10  and  m  the  motor  data  of  the  probability  distribution 
function.  All  other  variables  and  functions  are  defined  as  follows: 


k  =  {s,  m},  x  =  {xi,x2,x3,  ...xn}  of  s  or  m  and  fa  =  {fik,  ay}  as  defined  in 
the  previous  section. 

Mean(Al)  calculates  the  mean  of  the  set  of  X,  fix 


Returns: 


.  ES*; 


1*1 


StdDev(X)  calculates  the  standard  deviation  of  the  set  X,  ax 
Returns: 


■£^[(Xi-Mean(X)) 


1*1-1 


Var(Al)  calculates  the  variance  of  the  set  X,  a\ 


Returns: 


E#=i  (Xi-Mean{X)) 
1*1-1 


Gauss(x;,  //,  a)  is  the  function  of  p(xi\fi,  a2 


Returns: 


exp(— 


(Xj-fl)2 


\/27VO 


SumGaussk(.x'j,  ns ,  7rm,  fis,  fim,  as,  am )  is  the  function  of  \fik,  crk) 

Returns:  ns  Guess (xi,/is,as)  +  nm  Guess(xj,  fim,  am) 

SumGausskTimesLog(xj,  ns,  nm,  ps,  fim,  as,  am)  is  the  function  of 

Efc  (<  Qi  >  ^g{nkp(xi\fa))) 

Returns:  q(xj,  ns,  n's,  p's ,  a's,  nm,  ps,  fim ,  as,  am)  x  log(7rsGuess(xj,  ps,  crs))  + 

q(Xj,  7TS,  7 Tm,  fimi  0"m,  7Tm,  fiSl  fim ,  (Ts,  am)  X  log(7TmGueSs(Xj,  fimi  &m) 
q (Xi,  7TS,  7 r'k,  fi’k,  a'k,  nm,  fis,  fim,  as,  am)  is  the  function  of  <  gf  > 

Returns:  7r^.Guess(xj,  p'k,  a'k) / (SumGaussk(./V,  Xi,ns,  irm,  fis ,  /im,  as,  am )) 
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•  sum<  q  >  ( k ,  x,  7 ts,  nm,  l^S)  l-^m)  &S-)  <,/40 

For  (i  —  1  to  |x|) 

sum  =  sum  +  q(k,  xh  vrs,  n'k,  n'k,  a'k,  7Tm,  fj,8,  fj,m,  as,  am) 

End  For 
Return:  sum 

•  sumqXi (k,  x,  ns,  nm,  /is,  [xm,  as,  am,  n'kl  n'k,  cr'k) 

For  (i  —  1  to  |x|) 

sum  =  sum  +  xt  x  q{k,  xh  ns,  nk,  n'k,  a'k,  nm,  na,  fj,m,  as,  am) 

End  For 
Return:  sum 

•  sumqXiMinusMu ( k ,  x,  7 rs,  7rm,  fj,s,  fj,m,  as,  am,  ir'k,  fj/k,  ak,  fa) 

For  (i  —  1  to  |x|) 

sum  =  sum  +  q(k,  xh  ns,  n'k,  n'k,  a'k,  7 rm,  /is,  /rm,  crs,  am)  x  (x{  -  (/ifc))2 
End  For 
Return:  sum 
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PerformEM(0,  k ,  x) 

Initialize  7Tk  to  some  random  numbers 
While  (error>  e) 

Perform  E-Step 
For  (N  =  1  to  |x|) 

xn  E-Step(0,  6',  x,  N ) 

End  For 
Perform  M-Step 

double  SumqS-s— sumq(x,  ns,  nm,  fj,s,  pm,  as,  am,  i r' ,  n's,  a's) 
double  SumqM<— sumq(x,  ns,  nm,  pSl  as ,  am ,  n'm,  p'm,  a'm) 
ns  SumqS/|x| 

7 Tm  <—  SumqM/|x| 

Es  <-  sumqXi(x,  vrs,  nm,  n„,  //m,  as,  am,  n's,  n'a,  a')/SumqS 

A im  <-  sumqXi(x,  7TS,  nm,  n„,  as,  crm,  Tr'm,  /x'm,  o^J/SumqM 

as2  <— sumqXiMinusMu (x ,  i rs,  nm,  fj,s,  Hm,  crs,  &m,  K ,  /-4  cr' ,  fis)/ SumqS 

ofn  sumqXiMinusMu (x ,  nSl  nm,  ns,  pm,  crs,  <jm,  n'm,  p'm,  a'm,  /Js)/SumqM 


error  <-  \ps  -  p's\  +  \pm  -  p’m 
End  While  Loop 


Figure  A. 6  Expectation  Maximization  Algorithm 


A. 4  Algorithm  to  Convert  Vectors  to  Angles 

To  calculate  s  from  each  ASR,  a  direction  for  each  line  within  an  ASR  is 
calculated  to  obtain  an  ASR  direction.  Let  A  denote  the  current  vector  and  B  the 
vector  that  comes  before  A.  Let  U  be  the  normal  vector  along  the  positive  x  axis 
of  the  cartesian  map  represented  by  the  current  ASR  and  V  be  the  complete  vertex 


Figure  A. 7  Calculating  a  value  for  sensor  data  from  the  abstracted  map. 
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